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ABSTRACT 

This  thesis  develops  a  method  of  supervisory  control  of  the 
five-degree-of-freedom  dexterous  right  arm  of  the  Beam  Assembly 
Teleoperator  (BAT). 

The  thesis  imposes  a  constraint  that  the  end  effector  of  the  right  arm 
must  move  in  a  straight  line.  The  joint  angles  are  used  to  transform  the 
position  of  the  end  effector  into  cylindrical  coordinates.  With  the 
initial  and  final  position  known,  an  equation  for  a  straight  line  is 
obtained  and  the  cylindrical  coordinates  of  the  end  effector  for  each  point 
on  the  straight  line  are  found.  The  cylindrical  coordinates  are  then 
transformed  back  into  joint  angles  for  each  point  and  the  joint  angles  are 
used  to  control  the  BAT'S  right  arm. 

The  method  of  moving  the  end  effector  in  a  straight  line  is 
implemented  in  two  ways.  The  first  implementation  uses  an  unknown  starting 
position  and  moves  to  a  known  finish  point.  Here  the  previously  developed 
BAT  software  is  used  to  input  the  starting  position  of  the  arm.  Then  the 
straight  line  path  method  uses  this  starting  point  and  drives  the  arm  to 
the  known  finish  point.  In  the  second  implementation,  a  method  is 
developed  in  which  a  set  of  points  is  remembered  by  the  BAT  and  the 
straight  line  path  method  is  used  to  drive  the  arm  from  any  start  point, 
through  all  the  stored  points  and  return  to  the  original  point.  Further 
extensions  using  this  method  are  discussed. 

An  appendix  contains  a  block  diagram  description  of  the  Beam  Assembly 
Teleoperator  and  the  source  code  for  the  software  developed  and 
implemented. 


Thesis  Supervisor:  David  L.  Akin 

Title:  Rockwell  International  Assistant  Professor  of 

Aeronautics 


-3- 


ACKNOWLEDGEMENTS 


I  would  like  to  sincerely  thank  Professor  David  Akin  for  all  the 
assistance  he  has  given  me.  He  was  always  available  to  help  guide  and 
educate  me  throughout  the  entire  thesis.  It  was  obvious  that  educating 
and  helping  students  is  his  utmost  concern. 

Very  special  thanks  to  John  Spofford  who  helped  teach  me  every¬ 
thing  I  know  about  the  BAT  and  never  once  made  me  wait  when  I  asked  for 
help.  Good  luck  in  the  future  with  all  your  work,  John. 

Many  thanks  also  go  to  everyone  in  the  Space  System  Laboratory 
including  those  members  now  gone  who  have  worked  on  the  BAT  project 
from  its  conception  (in  particular,  Tom  Waschura  for  his  work  on  the 
software  and  Eric  Shain  for  his  work  on  the  wrist).  If  it  were  not 
for  their  hard  work  and  education  this  thesis  would  have  never  been 
conceived. 

Thanks  to  Homayoon  Kazerooni  for  his  guidance  concerning  M.I.T. 
and  the  Mechanical  Engineering  Department.  Thanks  for  taking  me  under 
your  wing  and  helping  me. 

I  would  also  like  to  thank  Sandy  Tepper  for  not  only  typing  my 
thesis  and  helping  me  out  from  the  start  of  my  stay  at  M.I.T.,  but  also 
for  knowing  more  about  M.I.T.  than  anyone  else. 

I  would  like  to  thank  the  U.S.  Army  for  making  it  possible  for  me 


to  obtain  this  degree. 


-4- 


Most  of  all,  I  would  like  to  thank  my  wife,  Susan,  and  my  children, 
Christopher  and  Lisa,  for  their  patience,  understanding  and  support.  I 
only  hope  I  can  repay  them  for  the  sacrifices  they  made  for  me  while  I 
was  at  M.I.T.  Thanks  for  always  being  there. 


-5- 


TABLE  OF  CONTENTS 


PAGE 

ABSTRACT  2 
ACKNOWLEDGEMENTS  3 
TABLE  OF  CONTENTS  ^ 
LIST  OF  FIGURES  ^ 


CHAPTER  1  - 

INTRODUCTION 

11 

CHAPTER  2  - 

A  METHOD  FOR  SUPERVISORY  CONTROL  OF  THE  RIGHT  ARM 

15 

2.1 

Objective 

15 

2.2 

Overview  of  the  Procedure 

17 

•  2.3 

Problem  Set-Up 

19 

2.4 

Joint  Angles  to  Cylindrical  Coordinates 

23 

2.5 

Straight  Line  Path  Generation 

29 

2.6 

Cylindrical  Coordinates  to  Joint  Angles 

36 

2.7 

Determination  of  the  Total  Time  and  the 

Values  of  the  Shoulder  Yaw  (e-i)  at  Each  Time  Step 

41 

2.8 

Determination  of  the 

Orientation  Angles 

51 

2.9 

Simulation 

52 

CHAPTER  3  - 

IMPLEMENTATION  I  : 

UNKNOWN  STARTING  POSITION, 

KNOWN  FINISH  POINT 

71 

CHAPTER  4  - 

IMPLEMENTATION  II: 

LEARNING  TRAJECTORIES  BY 
CONCATENATION 

89 

CHAPTER  5  - 

CONCLUSIONS 

103 

REFERENCES 

107 

APPENDIX  A 

-  DESCRIPTION  OF  THE 

BAT 

108 

APPENDIX  B 

-  PROGRAM  FOR  SUPERVISORY  CONTROL 

147 

-6- 


LIST  OF  FIGURES 

NUMBER 

TITLE 

PAGE 

1.1 

Sketch  of  the  ICS  and  BAT 

12 

2.1 

Diagram  of  the  Right  Arm  Control  System 

16 

2.2 

Joint  Angles 

19 

2.3 

Point  A  on  the  Right  Arm 

20 

2.4 

Coordinate  Frames  for  the  Right  Arm 

21 

2.5 

Coordinate  Frames  for  the  Wrist 

21 

2.6 

Cylindrical  Coordinate  Reference  Frame 

23 

ZJ 

Joint  Angles  to  Cylindrical  Coordinates  - 
Position  i 

24 

2.8 

Joint  Angles  to  Cylindrical  Coordinates  - 
Position  ii 

26 

2.9 

Joint  Angles  to  Cylindrical  Coordinates  - 
Position  iii 

28 

2.10 

Straight  Line  Path  in  Reference  Frame  (Xq,Yq,Zq) 

30 

2.11 

Projected  Line  in  XqYq  Plane 

30 

2.12 

Travelling  Across  the  Projected  Line 

31 

2.13 

Slope  of  the  Projected  Line  Equal  to  1 

35 

2.14 

Cylindrical  Coordinates  to  Joint  Angles  -  Case  i 

37 

2.15 

Cylindrical  Coordinates  to  Joint  Angles  -  Case  ii 

39 

2.16 

Cylindrical  Coordinates  to  Joint  Angles  -  Case  iii 

41 

2.17a 

Driving  Function  (Initial  Less  than  Final) 

42 

2.17b 

Driving  Function  (Initial  Greater  than  Final) 

43 

-7- 


NUMBER  title  page 

2-18  Simplified  Model  of  Arm  and  Beam  46 

2.19  Sampled  Driving  Function  49 

2.20  Arm  Position  -  Simulation  I  55 

2.21  Shoulder  Yaw  Position  -  Simulation  I  56 

2.22  Shoulder  Yaw  Velocity  -  Simulation  I  56 

2.23  Shoulder  Yaw  Acceleration  -  Simulation  I  57 

2.24  Wrist  Yaw  Position  -  Simulation  I  57 

2.25  Wrist  Roll  Position  -  Simulation  I  58 

2.26  Y  vs.  X  Position  of  Point  A  -  Simulation  I  58 

2.27  Z  vs.  X  Position  of  Point  A  -  Simulation  I  59 

2.28  Z  vs.  Y  Position  of  Point  A  -  Simulation  I  59 

2.29  Arm  Position  -  Simulation  II  60 

2.30  Shoulder  Yaw  Position  -  Simulation  II  61 

2.31  Shoulder  Yaw  Velocity  -  Simulation  II  61 

2.32  Shoulder  Yaw  Acceleration  -  Simulation  II  62 

2.33  Wrist  Yaw  Position  -  Simulation  II  62 

2.34  Wrist  Roll  Position  -  Simulation  II  63 

2.35  Y  vs.  X  Position  of  Point  A  -  Simulation  II  63 

2.36  Z  vs.  X  Position  of  Point  A  -  Simulation  II  g4 

2.37  Z  vs.  Y  Position  of  Point  A  -  Simulation  II  54 

2.38  Arm  Position  -  Simulation  III  55 

2.39  Shoulder  Yaw  Position  -  Simulation  III  66 


-8- 


NUMBER  title  ZMi 

2.40  Arm  Radius  Velocity  -  Simulation  III  66 

2.41  Arm  Radius  Acceleration  -  Simulation  III  67 

2.42  Wrist  Yaw  Position  -  Simulation  III  67 

2.43  Wrist  Roll  Position  -  Simulation  III  68 

2.44  Y  vs.  X  Position  of  Point  A  -  Simulation  III  68 

2.45  Z  vs.  X  Position  of  Point  A  -  Simulation  III  69 

2.46  Z  vs.  Y  Position  of  Point  A  -  Simulation  III  69 

3.1  Finish  Point,  Implementation  I  72 

3.2  Arm  Position,  Step  1,  Implementation  I  81 

3.3  Shoulder  Yaw  Position,  Step  1,  Implementation  I  82 

3.4  Wrist  Yaw  Position,  Step  1,  Implementation  I  82 

3.5  Wrist  Roll  Position,  Step  1,  Implementation  1  83 

3.6  Y  vs.  X  Position,  Step  1,  Implementation  I  83 

3.7  Z  vs.  X  Position,  Step  1,  Implementation  I  84 

3.8  Z  vs.  Y  Position,  Step  1,  Implementation  I  84 

3.9  Shoulder  Yaw  Position,  Step  2,  Implementation  I  85 

3.10  Wrist  Yaw  Position,  Step  2,  Implementation  I  86 

3.11  Wrist  Roll  Position,  Step  2,  Implementation  I  86 

3.12  Y  vs.  X  Position,  Step  2,  Implementation  I  87 

3.13  Z  vs.  X  Position,  Step  2,  Implementation  I  87 

3.14  Z  vs.  Y  Position,  Step  2,  Implementation  I  88 

4.1  Implementation  II  91 

4.2  Arm  Position,  Step  1,  Implementation  II  96 


-9- 


NUMBER 

TITLE 

PAGE 

4.3 

Shoulder  Yaw  Position,  Step  1,  Implementation  II 

96 

4.4 

Wrist  Yaw  Position,  Step  1 

,  Implementation  II 

96 

4.5 

Wrist  Roll  Position,  Step 

1,  Implementation  II 

97 

4.6 

Y  vs .  X  Position,  Step  1 , 

Implementation  II 

97 

4.7 

Z  vs.  X  Position,  Step  1 , 

Implementation  II 

98 

4.8 

Z  vs.  Y  Position,  Step  1 , 

Implementation  II 

98 

4.9 

Shoulder  Yaw  Position,  Step  2,  Implementation  II 

99 

4.10 

Wrist  Yaw  Position,  Step  2 

,  Implementation  II 

100 

4.11 

Wrist  Roll  Position,  Step 

2,  Implementation  II 

100 

4.12 

Y  vs.  X  Position,  Step  2, 

Implementation  II 

101 

4.13 

Z  vs.  X  Position,  Step  2, 

Implementation  II 

101 

4.14 

Z  vs.  Y  Position,  Step  2, 

Implementation  II 

102 

A.l 

Sketch  of  BAT 

108 

A. 2 

Main  Power  Subsystem 

no 

A. 3 

Control  Subsystem 

112 

A. 4 

Pneumatic  Subsystem 

113 

A. 5 

Video  Subsystem 

115 

A. 5 

Left  Arm  Subsystem 

117 

A. 7 

Right  Arm  Subsystem 

118 

A. 8 

Propulsion  Subsystem 

120 

A. 9 

Sketch  of  ICS 

121 

A. 10 

ICS  Control  Panels 

123 

A. 11 

Central  Controller 

126 

As  stated  previously,  the  concern  in  this  chapter  is  to  develop  a 
method  of  generating  a  straight  line  path  for  point  A.  In  this  chapter 
the  values  of  (should  yaw,  shoulder  pitch,  elbow,  wrist 

motor  A,  wrist  motor  B)  are  given  as  part  of  the  problem,  -^ij^  and  j/g 
will  be  tucked  away  until  Section  2.8  (since  the  motor  angles  do  not 
affect  the  position  of  point  A). 

At  this  time,  the  starting  position  of  point  A  is  represented  in 
terms  of  61,82,62  ( shoul der yaw,  shoulder  pitch,  elbow)  and  the  length  of 
the  two  links  (Li  and  12).  It  would  be  easier  to  compute  a  straight  line 
path  if  the  joint  coordinates  (6i,82,92>  ^i ,  L2)  were  changed  to  another 
coordinate  system. 

2.4  Joint  Angles  to  Cylindrical  Coordinates 

Cylindrical  coordinates  6,R,Z  (Figure  2.6)  were  chosen  because  of 
both  the  design  of  the  manipulator  and  the  ease  of  transformation.  Shoulder 


FIGURF  2.6  Cylindrical  Coordinate  Reference  Frame 
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formation  is  wrist  roll,  9^  .  8^  rotates  about  the  Y"  axis  from 

0  =  -1.745  radians  to  =  1.745  radians.  There  is  one  other 

^MIN  MAX 

consideration  concerning  the  wrist  angles.  When  the  wrist  was  developed 
by  the  Space  Systems  Laboratory,  a  differential  mechanism  and  two  motors 
(motor  A  and  motor  B)  were  used  inside  the  wrist  to  enable  it  to  have  yaw 
and  roll.  A  detailed  description  of  the  mechanism  is  described  in  refer¬ 
ence  [3].  The  angles  which  are  used  in  the  control  system  are  the  angles 
from  the  wrist  motors;  these  motors  are  labelled  A  and  B.  In  this  thesis 
will  be  used  as  the  angle  for  motor  A  and  will  be  used  as  the  angle 
for  motor  B.  Reference  [3]  develops  the  following  transformation  used  to 
convert  between  and  6^,  (the  two  motor  angles  and  the  two  . 

wrist  angles). 

•4  "  ■  I  '4  ^  \  ’^5 


^5  '  '  2a  ®4  ■  2a  ®5 


v^here  a  =  i  and  is  based  on  the  gear  ratios  of  the 

differential  drive. 


A1  so , 


-  84  -  a  65 


“^4  ■  ^  *-’5 
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arrow  in  each  frame  in  Figure  2.4  signifies  the  0  radian  point.)  The  second 

rotation  transformation  is  about  the  X.|  axis.  The  shoulder  pitch  or  62 

can  rotate  from  62  =  1-31  radians  to  2.62  radians.  The  coordinate  frame 

is  then  translated  along  the  Y2  axis  by  the  length  of  the  first  link 

(L.|  =  .41  meters).  The  elbow  rotation,  0^  ,  is  the  next  transformation 

It  rotates  about  X.,  from  9.,  =  .52  radians  to  9-  =  3.75  radians. 

-^MIN  -^MAX 

The  last  transformation  is  a  translation  along  the  axis  the  length 

of  the  second  link  (L2  =  -36  meters).  This  is  point  A. 

The  generation  of  a  straight  line  path  for  point  A  is  the  main 

concern.  However,  the  orientation  angles  (wrist  yaw  and  wrist  roll,  0^ 

and  0^)  can  also  be  changed  while  the  arm  is  moving.  The  reference  frame 

of  the  wrist  is  fixed  to  point  A  and  shown  in  Figure  2.5.  The  first 

transformation  is  wrist  yaw,  0^  ,  which  rotates  about  the  Z'  axis  and 

=  0  radians  to  0.  =  3.14  radians.  The  next  trans- 

MIN  ^MAX 


varies  from 
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FIGURE  2.2  JOINT  ANGLES 


2.3  Problem  Set^ 


The  position  of  the  end  effector  is  the  main  concern.  This  is  the 
point  that  must  follow  a  straight  line  path  (Point,  A,  Figure  2.3).  This 


gives  the  arm  three  degrees  of  freedom  (shoulder  yaw,  shoulder  pitch  and 
elbow).  The  restriction  of  point  A  to  follow  a  straight  line  path  from 
the  start  point  to  the  finish  point  is  a  two-link  problem.  The  reference 
frame  (X^  ,  ,  Zq)  is  positioned  at  the  base  of  the  first  link  (Point  0 

in  Figure  2.3).  The  first  rotation  transformation  is  about  the  Zq  axis 


(Figure  2.4).  This  is  shoulder  yaw  or  .  It  can  rotate  from 

=  0.0  radians  to  9,  =  2.44  radians.  (Note:  the  base  of  the 

^MIN  'max 
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From  this  the  central  controller  computes  the  initial  position  of  the  end 
effector.  The  procedure  for  converting  the  voltage  inputs  to  joint  angles 
will  be  covered  in  the  next  chapter.  The  final  position  is  predetermined 
in  advance  and  stored  in  the  central  controller.  The  problem  is  to  find 
a  trajectory  for  the  end  effector  of  the  arm,  compute  the  joint  angles,  and 
send  them  to  the  control  system.  The  procedure  for  sending  the  joint 
angles  to  the  control  system  will  also  be  covered  in  the  next  chapter. 
Additionally,  the  position  of  the  end  effector  is  restricted  to  following 
a  straight  line  path.  This  restriction  is  added  to  made  it  easier  to 
concatenate  paths  (path  concatenation  is  developed  in  Chapter  4). 

The  problem  is  now  in  the  following  format. 

ASSUMPTIONS: 

The  reference  frame  is  fixed  to  the  BAT  and  stationary. 

GIVEN; 

1)  Joint  angles  for  the  original  arm  configuration. 

(Shoulder  Yaw  (»i^ ) ,  Shoulder  Pitch  ,  Elbow  ("5^), 

Wrist  Yaw  {n^)  and  Wrist  Roll  (-j^).  Figure  2.2). 

2)  The  final  position  of  the  arm. 

3)  The  physical  dimensions  of  the  arm. 

FIND: 

1)  A  straight  line  path  for  the  position  of  the  end 
effector  from  the  start  point  to  the  finish  point. 

2)  The  joint  angles  for  each  position  on  the  path. 
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determine  what  conf iguration  the  arm  is  in,  and  start  generating  inputs 
to  the  control  system  for  a  specific  task.  These  inputs  would  mimic  the 
inputs  that  would  have  come  from  the  master  arm  if  the  master  were  con¬ 
trolling  the  task.  An  important  task  in  an  assembly  operation  is  for  the 
arm  to  move  from  a  point  (x,y,z)  (in  a  three-dimensional  space  with  a 
reference  frame  stationary  and  fixed  to  the  BAT)  to  another  point  (x,y,z). 
This  thesis  develops  a  method  for  the  central  controller  to  control  the 
right  arm  moving  from  any  point  to  a  specified  point.  Using  this  method, 
it  develops  a  way  to  concatenate  specified  points  and  move  the  arm  through 
these  points.  This  will  enable  the  BAT  to  learn  many  trajectories  which 
can  be  combined  to  perform  assembly  tasks.  This  will  be  the  basis  for 
the  extensions  to  other  types  of  tasks. 

2.2  Overview  of  the  Procedure 

In  this  chapter,  the  task  is  to  develop  a  method  in  which  the  arm 
moves  to  a  specified  location;  for  example,  the  operator  has  just  grasped 
a  beam  with  the  right  arm,  and  would  like  to  move  to  a  location  next  to 
the  left  arm.  The  central  controller  must; 

1)  know  the  starting  configuration  of  the  arm; 

2)  know  the  final  configuration  of  the  arm; 

3)  compute  a  trajectory; 

4)  transmit  the  appropriate  command  to  the 
joint/actuator  control  system. 

The  operator  starts  this  procedure  by  entering  a  keyboard  option  on 
the  control  panel  at  the  ICS.  This  would  let  the  central  controller  know 
that  the  supervisory  control  option  has  been  selected.  The  original  con¬ 
figuration  is  then  read  from  the  inputs  of  each  joint  of  the  master  arm. 


MASTER 

ARM 


Right  Arm  Control  System  | _  ENCODER  ENCODER 

INTERFACE  INTERFACE 
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CHAPTER  2 

SUPERVISORY  CONTROL  OF  THE  RIGHT  ARM 


2.1  Objective 

Using  the  two  joysticks  to  control  the  BAT's  position  and  the 
master  arm  to  control  the  BAT's  right  arm,  it  is  easy  to  imagine  times 
where  controlling  the  operation  of  the  BAT  can  become  a  very  hard  task 
with  only  two  hands.  The  operator  might  also  need  to  enter  an  option 
in  the  BAT's  software.  The  control  system  of  the  right  arm  can  be 
improved,  so  that  the  master  arm  is  not  needed  to  perform  most  tasks. 

There  are  some  cases  in  an  assembly  operation  where  the  master-slave 
relationship  would  be  easier  to  use.  (For  example,  if  two  beams  do  not 
fit  just  right  and  have  to  be  jiggled  to  make  the  connection.)  However, 
in  most  cases  it  would  be  much  easier  to  simply  enter  a  command  and  let 
the  central  controller  move  the  right  arm  to  a  preprogrammed  position. 

Figure  2.1  depicts  the  entire  control  system  of  the  BAT's  right 
arm.  The  position  of  the  right  arm  is  fed  bacx  only  to  the  joint/ 
actuator  cards  and  not  to  the  central  controller.  Therefore,  the  central 
controller  must  receive  the  positions  of  the  right  arm  from  the  master 
arm  at  the  ICS.  The  assumption  is  made  that  the  master  arm's  joint 
angles  are  equal  to  the  right  arm’s  joint  angles,  or  at  least  the  relation¬ 
ship  is  a  known  linear  one  (recall  the  master  arm  is  kinematically  similar 
to  the  right  arm).  In  the  supervisory  mode,  the  commands  from  the  master 
arm  are  halted  at  the  central  controller.  The  central  controller  can  then 


with  a  specified  start  and  finish  point,  the  BAT  system  will  then  be 
taught  to  not  only  remember  the  trajectory,  but  also  to  play  it  back 
at  the  operator's  command.  This  will  give  the  BAT  system  the  capability 
of  performing  any  task  over  and  over  again  without  the  use  of  the  master 
arm.  There  are  many  tasks  that  are  repetitive  during  an  assembly  opera¬ 
tion.  It  would  not  be  an  efficient  use  of  the  BAT  system  if  the  operator 
were  to  perform  these  tasks  again  and  again  using  the  master  arm  at  the 
ICS.  By  upgrading  the  control  system,  the  BAT's  capabilities  will  be 
greatly  enhanced.  Freeing  the  operator’s  hands  will  further  increase 
the  man-machine  productivity. 
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degrees  of  freedom.  It  is  also  equipped  with  two  cameras  providing  the 
operator  with  two  different  perspectives  of  the  work  area. 

The  BAT  is  controlled  by  the  ICS.  The  ICS  has  two  three  degree-of- 
freedom  joysticks  to  control  the  BAT's  six  maneuvering  degrees  of  freedom. 
There  is  a  master  arm  which  is  kinematically  similar  to  the  BAT's  dexter¬ 
ous  right  arm.  The  operator  uses  this  master  arm  to  control  the  BAT's 
right  arm  in  a  master-slave  control  relationship.  On  the  ICS  there  is 
also  a  helmet  gimbal  system  used  to  control  the  tilt  and  pan  unit  of  the 
BAT's  main  camera.  The  BAT  and  the  ICS  are  described  in  further  detail 
in  Appendix  A. 

To  perform  an  assembly  task  with  the  system,  the  operator  uses  the 
two  joysticks  to  position  the  BAT  at  the  work  area.  The  operator  then 
straps  his/her  right  arm  to  the  ICS's  master  arm  and  positions  it  to  where 
he/she  would  like  the  BAT's  right  arm  to  be.  The  left  arm  is  used  to  hold 
the  workpiece  in  place.  This  can  be  quite  challenging  especially  while 
the  operator  is  trying  to  control  the  BAT's  position  with  the  joystick 
controls.  A  method  of  freeing  the  operator  from  the  master-slave  control 
relationship  would  help  the  man-machine  to  be  more  productive. 

In  this  thesis,  the  operator  is  released  from  certain  tasks  by 
implementing  supervisory  control  of  the  right  arm.  Two  different  tra¬ 
jectories  will  be  used  to  demonstrate  the  control.  The  first  trajectory 
will  have  an  unknown  starting  point  and  a  specified  finishing  point.  By 
selecting  a  keyboard  option,  the  operator  will  be  able  to  move  the  BAT's 
right  arm  to  this  specified  finish  point.  By  defining  a  second  trajectory 
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a  more  efficient  and  effective  system.  This  thesis  will  continue  along 
this  trend  to  upgrade  the  interface  between  man  and  machine. 

The  teleoperator  system  is  made  up  of  two  major  structures: 

1)  the  Beam  Assembly  Teleoperator  (BAT) 

2)  the  Integrated  Control  Station  (ICS) 


FIGURE  1.1  Sketch  of  the  ICS  and  the  BAT 

The  BAT  has  a  fixed  left  arm  and  a  dexterous  right  arm.  The  left 
arm  has  a  gripper  end  effector  used  for  grasping  a  beam,  and  a  motor- 
driven  roller  used  for  moving  the  beam  laterally.  The  right  arm  is  a 
five  degree-of-f reedom  manipulator  with  a  two- jaw  grip  end  effector.  The 
BAT  frame  has  eight  trolling  motors  which  are  used  to  give  the  unit  six 
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CHAPTER  1 
INTRODUCTION 

One  of  the  most  important  parameters  affecting  the  industrialization 
of  space  is  the  capability  to  assemble  structures  in  orbit.  Much  larger 
structures  can  be  built  in  space  than  on  earth.  Since  there  is  a  limit 
to  payload  capacity,  the  need  to  assemble  structures  in  space  is  impera¬ 
tive,  Experimentation  with  assembly  tasks  on  earth  will  help  in  the 
development  of  the  most  efficient  assembly  method,  which  will  increase 
productivity  on  an  actual  mission.  Tests  have  already  been  conducted, 
in  a  simulated  environment,  evaluating  the  ability  of  humans  to  assemble 
objects  in  space.  The  next  step  is  to  see  how  a  machine  can  help  man  in 
accomplishing  these  tasks.  The  Space  Systems  Laboratory  at  M.I.T.  has 
already  conducted  tests  involving  assembly  by  humans  in  a  simulated 
environment.  Neutral  buoyancy  was  used  as  the  simulation  medium.  A 
teleoperated  system  was  also  developed  which  will  simulate  the  man- 
machine  interface  in  space.  This  device  is  called  the  Beam  Assembly 
Teleoperator  (BAT).  The  BAT  operates  in  the  neutral  buoyancy  environ¬ 
ment. 

The  Space  Systems  Laboratory  began  development  of  the  BAT  in  mid 
1981,  and  has  been  using  this  device  in  experiments  with  man-machine 
assembly  tasks.  The  software  for  master-slave  control  of  the  BAT 
manipulator  was  previously  developed  in  the  Space  Systems  Lab.  Both 
the  hardware  and  the  software  are  being  continuously  improved  to  provide 
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yaw  (e-j)  transforms  into  c  identically.  The  transformation  of  shoulder 
pitch  and  elbow  (e^  and  e^)  to  R,Z  is  just  a  planar  problem.  The  plane 
is  defined  by  the  location  of  d  .  For  a  particular  value  of  d  the  arm 
can  only  be  in  these  positions. 


i )  Elbow  angle  (e^) 

i i )  Elbow  angle  (e^) 

iii )  Elbow  angle  (e^) 


(Elbow  down  position) 
(Elbow  up  position) 
(Fully  extended) 


Position  i) 
Note: 


Elbow  angle  (e^)  <  n  (Figure  2.7) 

There  can  never  be  a  negative  R  so  the  arm  is  only 
in  Quadrant  I  and  IV  of  the  (R,Z)  plane. 


FIGURE  2.7  Joint  Angle  to  Cylindrical 


Coordinates  -  Position  i) 
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To  find  Z,R  for  either  of  the  two  diagrams  in  Figure  2.7  the  followi 
procedure  is  used: 

1)  find  Y  (the  distance  from  the  origin  to  point  A) 
using  the  law  of  cosines. 


Y  =  /  ~  2xLixL2xcos  0^ 

2)  find  0  (the  angle  of  the  triangle  formed  by 
and  L-i  at  the  origin)  using  the  law  of  sines, 

1  L,  sin  9, 

4  =  si.-'  (  i  ) 


(NOTE:  0  can  only  be  between  0  and  tt/2  radians, 

therefore,  there  is  no  ambiguity  in  the  arcsin.) 


3)  find  0  (the  angle  between  y  and  the  R  axis) 


Y  2  ”  (02”  1^^ 


4)  find  Z,R  using  polar  coordinate  transformations 


Z  =  sin  0 


R  =  »Y‘  -  r- 


Position  ii)  Elbow  angle  (B^)  > 


(Figure  2.8) . 


FIGURE  2.8  Joint  Angle  to  Cylindrical  Coordinates  -  Position  ii 


This  configuration  of  the  master  arm  is  not  possible  given  the  current  design 
of  both  the  master  arm  of  ICS  and  the  dexterous  arm  of  BAT.  It  is 
included  in  this  thesis  to  ensure  that  this  method  can  handle  all  types 
of  inputs  to  the  control  system.  (The  algorithm  for  finding  R  and  Z 
is  similar  to  Position  i). 
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1)  Find  Y  (the  distance  from  the  origin  to  point  A) 
using  the  law  of  cosines 

Y  =  /  L|  +  -  2  X  X  X  cos(2tt  -  0^) 

(NOTE;  The  angle  of  the  triangle  is  now  Z-r  -  6^  instead  of  e^. 

2)  find  0  (the  angle  of  the  triangle  formed  by  y  ,  L2 
at  the  origin)  using  the  law  of  sines 

T  sin(2-^  -  0,) 

0  =  sin-'  (  ^ ^  ) 

(NOTE;  can  only  be  between  0  and  tt/2.) 

3)  find  tjj  (the  angle  between  y  and  the  R  axis) 

^  ~  '2  ~  (®2  ~ 

4)  find  Z,R 

Z  =  YSinii>  R  =  v'y^  -  Z" 
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Upon  receiving  the  joint  angles  (shoulder  yaw,  6^  ,  shoulder  pitch, 

0^  .  and  elbow,  S^)  the  controller  checks  6^  and  finds  9,R,Z  of  the 
initial  position  of  point  A  from  one  of  these  three  algorithms. 

In  this  chapter  the  final  position  of  point  A  is  known  and  has 
been  stored  in  the  controller.  The  final  position  is  stored  in  cylindri¬ 
cal  coordinates.  The  final  orientation  angles  9^,  9^  (wrist  yaw  and 
wrist  roll)  are  also  stored  and  are  in  radians.  The  controller  knows  it 
must  move  point  A  from  e,R,Z  (initial)  to  e,R,Z  (final)  in  a  straight 
line. 

2.5  Straight  Line  Path  Generation 

If  a  path  is  a  straight  line  in  Xq>Yq,Zq  space,  it  can  be  projected  down  to 
a  straight  line  in  the  X,y  plane  (Fig.  2.10).  Due  to  the  maximum  and  minimum 
values  of  shoulder  yaw  (9^)  only  the  first  and  second  quadrants  of  the 
(Xq,Yq)  plane  are  involved.  Having  e,R  initial  and  6,R  final, 
measured  as  shown  in  Figure  2.11,  the  values  for  X,Y  initial  and  X,Y 
final  are  easily  computed. 


Initial 


Final 


Y .  =  R .  X  sin  9 . 

1  1  ^ 


Yf  =  R^  X  sin  9^ 


X .  =  R .  X  cos  6  . 

1  1  1 


X^  =  R^  X  sin 


From  here  the  equation  for  a  straight  line  is  then  computed  in  terms  of 


X  and  Y. 
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Y  =  m(X  -  X.)  +  Y. 


(2.1) 


where 


[The  case  when  aX  =  0  sets  the  flag.]  The  value  for  e 
changes  every  time  step.  The  method  for  changing  9  (also  shoulder 
yaw,  8^)  will  be  discussed  in  Section  2.7.  Assume  for  now  that  the 
value  of  9  at  each  time  step  is  given.  Then  R  and  Z  are  computed 
using  the  X  and  Y  computed  from  the  straight  line  restriction  (2.1). 
(See  Figure  2.12.) 


FIGURE  2.12  Travelling  Across  the  Projected  Line 
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To  review  what  has  been  done  until  now,  first  the  joint  angles 
(e-j, 62*^3)  were  obtained  from  the  arm.  The  joint  angles  were  then 
converted  to  cylindrical  coordinates  of  point  A.  With  the  initial  and 
final  cylindrical  coordinates  of  point  A,  a  straight  line  path  in  the 
XqjYq.Zq  plane  was  found  for  point  A.  As  e  (which  is  also  the 
shoulder  yaw  angle,  )  sweeps  across  the  straight  line,  the  value  of 
R  and  Z  must  be  found  for  each  e  .  This  will  give  the  cylindrical 
coordinates  of  point  A  on  the  straight  line.  Knowing  the  value  of  6  , 

R  and  Z  of  point  A,  this  will  be  used  to  convert  back  to  joint  angles 
(this  part  will  be  covered  in  Section  2.6).  To  find  the  values  of  R 
and  Z  for  each  0  ,  four  special  cases  may  be  examined. 

Case  1)  AG,  AZ  =  0 

Here,  6  and  Z  retain  the  value  of  6^  and  Z^  . 
Depending  on  whether  is  less  than  or  greater 

than  R^  ,  the  appropriate  driving  function  is  used 
to  determine  R  at  each  time  step  (this  driving 
function  is  the  same  one  used  to  determine  6  when 
A0  ^  0  and  will  be  covered  in  Section  2.7). 

Case  2)  Ad,  AR  =  0 

This  is  done  exactly  as  above  except  that  R  is  set 
equal  to  R^  and  Z  is  used  in  the  driving  function. 

Case  3)  A6  =  0 

For  this  condition,  R  is  used  in  the  driving  function. 

X  and  Y  are  found  using  simple  conversions. 
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X  =  R  X  cos  0 
Y  =  R  X  sin  0 

Dependent  upon  the  value  of  AX  and  AY  the 
appropriate  side  of  the  equation  for  a  straight 
line  (shown  below)  is  used  to  compute  Z  . 


X 

1 

X 

11 

1 

-  z. 

1 

Y  - 

Y. 

1 

Xf 

-  X. 

Zf 

-  z. 

1 

1 

-< 

1 

Y. 

1 

Case  4)  If 

AR, 

AZ  = 

0 

or 

AR 

=  0 

or 

AZ 

=  0 

or 

Ae, 

AR,  AZ 

/  0 

the  following  procedure  is  used. 


The  value  of  each  9  ,  given  by  the  driving  function,  is  used 
to  compute  the  next  X  and  Y  along  the  straight  line.  From  these 
two  values,  R  is  computed  as  shown  below. 

General  case: 
using 


Y 


X 


tan  0 
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substitute  this  into  the  equation  for  a  straight 
line  (2.1) 


Y  =  m(  — ”  -  X.)  +  Y 

'  tan  D  1  1 


Then,  solving  for  Y 


(Y.  -  m  X.) 


(1 


m 


tan  6 


Now  with  Y  and  5,  find  X 


tan  a 


R  is  then  found  by 


R  =  v'  X^  +  Y‘- 


Flagged  cases: 

is  checked  as  it  changes,  so  that  when  the  next  -  reaches 
a  value  of  ,  (Figure  2.12,  in  this  case  =  1.53  radians) 

retains  this  value  until  it  passes  r  =  •'/2  .  At 


-/2  , 


is  then  set  to 


high 


which  is  1.60  radians.  This 


is  to  account  for  the  singularity  at 


There  are  four 


more  cases  which  are  flagged. 


Case  1)  If  -x 


0 


(Figure  2.13) 


FIGURE  2.13  Slope  of  the  Projected  Line  Equal  to  1 


Then, 


Y  =  X.  tan  e 
X  =  X. 


Case  2)  If  m  =  0 


Then 


X 


Y. 

1 


tan  9 


Y  =  Y. 

1 

Case  3)  If  9  =  0 
Then 


Y  =  0 


X 


Y. 

X.  -  —  (except  when  m  =  0) 
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Case  4)  If  (possibly  as  a  starting  point) 

Then 

X  =  0 

Y  =  Y.  -  m  X. 

1  1 


Once  X  and  Y  are  known,  using  the  general  or  flagged  cases,  R  is 
then  found  using 


R  =  .X-  +  Y‘ 

To  find  Z  ,  the  equation  for  a  straight  line  in  three-dimensional 
space  is 


X  -  X. 

Z  -  z. 

-< 

1 

1 

1 

1 

X.  -  X. 

Z.  -  z. 

Y.  -  Y. 

f  1 

f  ^ 

f  1 

values  of  Z.  and 

Z^  are  known 

and  depending  on  whether 

is  zero  the  right 

or  left  side  of 

the  equation  is  used. 

^  )  (Zf  -  zp  -Z,  Z  =  (  )  (Z,  -  z.)  .  z, 

2.6  Cylindrical  Coordinates  to  Joint  Angles 

At  this  point,  the  values  of  ".R.Z  which  keep  point  A  on  a 
straight  line  path  are  known.  Therefore  for  a  particular  value  of  , 


finding  (shoulder  pitch)  and  (elbow)  is  a  planar  problem. 

Recall  that  in  cylindrical  coordinates  is  equal  to  (shoulder 

yaw).  The  algorithm  to  find  the  joint  angles  is  simply  the  reverse  of 
going  from  (6-1,02503)  joint  angles  to  (6,R,Z)  cylindrical  coordinates. 
Only  three  positions  can  occur  here  also. 

Position  i)  Elbow  angle  (9^)  <  t  (figure  2.14) 


FIGURE  2,14  Cylindrical  Coordinates  to  Joint  Angles  -  Case  i 

1 )  find  : 


vR-  +  Z- 
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If  the  case  arises  where  .IR  =  0  (case  2,  Section  2.5),  T  is 
again  computed  using  the  maximum  id  =  The  driving  function  will  now 
be  Z(t). 


Z(t)  =  Z.  +  (;.Z(  I  -  ^  sin(2-:  I  ))) 

Zf  ■ 

Z(t)  =  Z^  -  (lZ(l  -  Y  +  p  Sin(2r  Y  ))) 


The  values  of  the  orientation  angles  (wrist  yaw  and  wrist 

roll)  do  not  affect  the  straight  line  path  of  point  A.  Therefore,  wrist 


yaw,  ,  and  wrist  roll,  ,  can  be  changed  slowly  as  the  arm  moves 
point  A  along  its  straight  line  path.  Recall  from  the  end  of  Section  2.3, 
and  (initial)  are  obtained  by  transforming  the  values  of  C4 

(wrist  motor  A)  and  (wrist  motor  B).  The  values  of  the  final  orienta¬ 
tion  angles,  and  ,  are  stored  along  with  the  final  position  of 

point  A.  In  order  to  have  all  of  the  joints  of  the  arm  finish  at  the  same 
time,  the  time  it  takes  to  go  from  (initial)  to 

(final)  will  be  T  (the  total  time  to  traverse  the  straight  line  path. 
Section  2.7).  To  account  for  gradual  acceleration  and  braking  of  the  two 
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increment,  will  be  the  sampling  time  of  the  arm's  control  system 
(At  ~  .05  seconds).  It  is  obvious,  by  looking  at  Figure  2.19,  that 
gradual  acceleration  and  gradual  braking  will  be  applied  to  the  shoulder 
yaw.  At  each  time  increment  the  values  for  t,T  and  Ar  ,  are  used 
in  the  driving  function  to  compute  the  value  of  ■l‘i{t)  .  This  value  of 
1  is  then  used  to  determine  R  and  Z  of  point  A  on  a  straight  line 
path  as  shown  in  Section  2.5. 

For  case  3,  when  ^  ,  the  driving  function,  A-jlt)  cannot 

be  used.  In  this  case  T  is  computed  using  the  maximum  change,  AA-j  =  ”. 
The  driving  function  will  then  be  determined  by  the  change  in  radius,  AR  . 
The  same  type  of  driving  function  is  used  for  R(t)  as  was  used  for 
^(t)  in  case  1  or  2,  This  will  also  provide  gradual  acceleration  and 
braking  of  the  ami. 

R,  R. 
f  1 

R(t)  =  R.  +  (AR(  I  -  ^  sin(2-  |  ))) 

R.  ■:  R. 
f  1 

R(t)  =  R^  -  (AR(1  -  Y  +  ^  sin(2-  y  ))) 

The  values  of  X,Y  and  Z  are  then  computed  as  in  case  3,  Section  2.5 
(also  for  case  1 ) . 
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small.  Too  small  a  time  might  cause  the  controller  to  generate  a  command 
that  could  saturate  the  actuator.  Therefore,  during  experimentation  with 
this  method  on  the  BAT,  the  value  of  FS  is  approximately  3.5.  It  starts 
out  this  high  for  equipment  safety  and  can  be  decreased  if  the  arm  moves 
too  slowly.  Once  the  value  of  Ad  (the  change  in  shoulder  yaw)  is  com¬ 
puted  from  the  initial  and  final  position  of  point  A,  it  is  used  to 
compute  T  ,  the  total  time  to  traverse  the  straight  line  path. 

The  value  of  T  along  with  the  value  of  A9  (the  shoulder  yaw 

change)  is  used  in  the  driving  function,  6(t)  (Figure  2.17).  By  lettin 

t  start  from  0  and  slowly  increasing  t  until  t  =  T  ,  the  driving 
function,  ■r(t),  will  be  sampled  [1].  This  is  shown  in  Figure  2.19  for 

the  case  when  is  greater  than  .  The  value. of  At,  the  time 


FIGURE  2.19  Sampled  Driving  Function 
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Integrate  over  X  to  find  the  total  drag  force.  To  find  the  Drag 
Moment, 


Drag  Moment  =  Drag  Force  x  Moment  Arm 

Drag  Moment  Arm  =  Y  ;  0, 

Drag  Moment  Beam  =  Dg^^j^Y  Y  :  +  Diameter  of  Beam 

Integrating  over  Y  gives  the  total  drag  moment  on  the  arm  and  beam  for 
a  given  velocity,  V  .  The  two  integrations  give  a  constant  (called  Drag) 
multiplied  by  the  9  term 

Drag  Moment  =  Drag 

The  value  for  maximum  torque  (t)  was  computed  using  the  maximum 
torque  for  the  shoulder  yaw  motor  and  the  gear  train  dimensions  between 
the  shoulder  and  the  arm.  To  find  the  total  time,  the  maximum  accelera¬ 
tion  is  used  along  with  the  velocity  at  that  time.  Recall  that  the  maxi- 
inum  acceleration  occurs  at  t  =  -^  .  The  value  for  t  is  substituted  into 
the  equations  for  acceleration  and  velocity  (Figure  2.17).  These  values  of 
acceleration  and  velocity  are  used  with  the  constants  (Jj,  A9,  Drag)  to  find 
the  equation  for  T  (Eq.  (2.2)).  FS  is  a  factor  of  safety  which  is  used 
to  ensure  that  the  time  taken  to  traverse  the  straight  line  path  is  not  too 


Moment  of  Inertia; 


"  ^ARM  '^BEAM 


^ARM  ^ 

^ARM  "  ^GA  ^  >RM  ^  T"  ^ 


1  2  2 

'^GA  ^  T2  "^ARM^^  ’^ARM  ^ARM 


'beam 


,  ,,  .  '^BEAM 

^GB  '^BEAM^  ARM  2  ^ 


1  T  n,  r2 

'^GB  ■  2  BEAM  BEAM 


Drag  Moment: 


Total  Drag  Force  = 

°ARM 

°BEAM 

where 

c  CpA 

D  2 

ARM 

Velocity 

=  Olx) 

X 

•  “-ARM 

A 

=  (Diameter) (X) 

X 

•  ’-ARM 

BEAM 

Veloci ty 

°  ■  *'-ARM  *  ** 

X 

:  0,  Diameter  of  Beam 

A 

X 

0,  Diameter  of  Beam 
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The  values  of  Jj  (total  moment  of  inertia  of  the  arm  and  beam) 
and  drag  moment  exerted  on  the  arm  and  beam  by  the  medium  are  constants 
and  computed  using  the  simplified  model  shown  in  Figure  2.18.  (The  drag 
moment  is  a  constant  when  using  the  value  for  velocity  at  T  =  -^  . 


FIGURE  2.18  Simplified  P^odel  of  Arm  and  Beam 

By  overestimating  the  dimensions  of  the  arm  and  the  beam,  and  assuming 
the  arm  and  beam  are  in  the  position  shown  in  Figure  2.18,  the  values 
of  Jj  and  the  drag  moment  are  also  overestimated.  These  two  constants 
were  easily  computed  using  very  straightforward  equations. 


the  path  is  not  of  the  utmost  importance.  The  main  concern  is  to  make 
sure  a  command  is  not  generated  which  will  saturate  the  actuator.  There¬ 
fore,  as  long  as  the  values  used  in  the  calculation  of  T  are  over¬ 
estimated,  the  arm  will  move  at  a  reasonable  speed  and  the  command  will 
not  saturate  the  actuator.  To  find  T  ,  the  equation  for  the  torque  and 
drag  force  is  used  and  solved  for  T  . 

T  =  J-j-e  +  Drag  Moment 

the  maximum  torque  applied  to  the  arm 

the  total  moment  of  inertia  of  the  arm 
grasping  a  beam 

the  total  drag  moment  on  the  beam  and  the  arm. 

at  t  =  ^ 


*2 

Substituting  and  solving  for  T 


where  F.S.  =  a  Factor  of  Safety. 


Case  1)  Shoulder  yaw  initial  (9,  )  <  shoulder  yaw  final  (O,  ). 

'i  'f 

Case  2)  Shoulder  yaw  initial  (0,  )  >  shoulder  yaw  final  (9,  )  . 

'i  'f 

The  velocity  and  acceleration  are  simply  the  first  and  second  derivatives. 
To  account  for  the  starting  position  not  being  at  zero  in  Figures  2.17a 
and  2.17b,  the  two  equations  are  adjusted  as  follows. 

Shoulder  yaw  initial  <  shoulder  yaw  final 

9l(t)  =  S-J  _  +  (j^e;  (  Y  ■  ^  sin(27T  Y  ))) 

Shoulder  yaw  initial  >  shoulder  yaw  final 

■9^(t)  =  9^^  +  (l/ieid  -  Y  +  ^  Sin(2-  ^  ))) 

Case  3,  when  r3-|.  =  ,  will  be  discussed  at  the  end  of  this  section. 

By  knowing  the  maximum  acceleration  the  motor  is  capable  of  pro¬ 
ducing  ur,  jer  the  worst  conditions,  T  (the  total  time  it  takes  the  arm  to 

traverse  the  path)  can  be  determined  for  a  value  of  3.9^  •  It  is  apparent 

T  3T  T 

that  the  maximum  acceleration  occurs  at  t  =  ^  and  t  =  -^  .  t  =  ^  will 
be  used  to  compute  T  ,  the  total  time.  Putting  t  =  ^  into  the  equations 
for  9i(t)  and  O-jCt)  ,  the  maximum  acceleration  and  velocity  at  that  time 

A9  and  T  .  In  this  thesis,  the  speed  to  complete 


are  now  functions  of 
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{t)< 


POSITION 


■  Ae 


8^{t)  =  Ae(l 


-  ^  TIME 

T 


e,(t) i 

'i/ 


T"^sin(2-T^  )) 


VELOCITY  AT  0 


1  __  .  ^TIME 


e^Ct) 


.16 


(1  -  cos(2tt 


ACCELERATION  AT  8^ 

i _ _ _ ^ . . -  TIME 

r  ./  T 

{ 

T  =  Total  Time 


9^(t) 


2tt-  A0 


sin(2-n  Y  ) 


FIGURE  2.17b  Driving  Function  (Initial  Greater  than  Final) 


— <1  rt" 
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VELOCITY  AT  6, 


T  =  total  time 


^(t)  =  (sin(2-  ^  ))  • 

T 


FIGURE  2.17a  Driving  Function  (Initial  Less  than  Final) 


FIGURE  2.16  Cylindrical  Coordinate  to  Joint  Angles  -  Case  iii 


2.7  Detennine  the  Total  Time  and  the  Value  of  Shoulder  Yaw  (■3i) 


Knowing  A  (initial)  and  8  (final)  from  the  end  of  Section  2.4, 
gives  the  initial  and  final  shoulder  yaw  angles.  The  motor  at  the  reference 
base  of  the  arm  drives  the  shoulder  yaw,  8^  .  The  specifications  of  this 
motor  and  a  simplified  model  of  the  arm  will  detennine  the  total  time  it 
takes  the  arm  to  traverse  the  straight  line  path.  With  the  initial  and 
final  value  of  U-j  ,  a  driving  function,  0-|(t)  ,  is  used  to  provide 
ample  braking  of  the  system  (Figure  2.17a  and  2.17b)  [2].  The  braking 
will  be  shown  later  when  this  function  is  sampled.  Three  cases  can  occur. 


3)  find  (elbow) 


2tt  _ 


cos"^ ( 


2  1,4 


Note:  The  angle  in  the  triangle  is  only  from  0  to  tt  . 

4)  find  ■? 


,  L„  sin(27T  -  9,) 

:  =  sin‘'(  ^ ^ ^  ) 

y  is  only  from  0  to  . 

5)  find  02  (shoulder  pitch) 

©2  =  ■  ('4'  4') 

Position  iii)  Elbow  angle  {6^)  =  (Figure  2.16) 

This  case  cannot  happen  in  a  straigl.t  line  path  unless  the 
controller  computes  a  y  greater  than  y  due  to  an  input  error  or 

if  the  manipulator  starts  out  with  9^  =  In  either  case,  the 
algorithm  switches  to  Position  i).  Through  simpi'^  trigonometric 
routines,  the  values  of  0pff2»'^^3  (shoulder  yaw,  shoulder  pitch  and 
elbow)  are  found  which  will  move  the  end  effector  along  a  straight  line 


Position  ii)  Elbow  angle  (S^) 


> 


(Figure  2.15) 


FIGURE  2.15  Cylindrical  Coordinates  to  Joint  Angles  -  Case  ii 

1)  find  Y 

7  =  v~Z‘  +  R-’ 


2)  find  7 

.  =  tan~^  (  I  ) 

.  is  only  from  -  '/2  to  “/2 
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Y  is  checked  here  to  insure  it  does  not  exceed  y 

'max 

This  is  just  in  case  the  operator  mistakingly  enters  in  a 
point  beyond  the  manipulator's  reach.  If  y  >  y^^^  , 

Y  is  used  and  6.,  (elbow)  will  be  at  u  . 

nia  X  o 

2)  find  ip 


ip  =  tan"^  (  I  ) 

Note;  >p  can  only  go  from  “  f  ^  »  therefore,  the 

arctan  function  will  have  no  ambiguity. 

3)  find  6^  ,  using  the  law  of  cosines  (elbow) 

(  2r|4  >  "-i  *  • 

Note:  The  value  of  3^  can  only  go  from  0  to  -t  so 

the  value  of  the  arc-cosine  function  will  have  no  ambiguity. 

4)  find  0  ,  using  the  law  of  sines 


Note;  The  value  of  ■;  can  only  be  between  0  and 
4)  find  9^  (shoulder  pitch) 


wrist  motors,  the  values  of  and  at  each  time  step  are  obtained 
using  the  same  type  of  driving  functions  as  G^(t)  .  For  example,  if 
^4f  ^  ®4i  ®5f  ^  ®5i  values  at  each  time  step  for  and 

6j-  will  be  found  using 

sj 

94(t)  =  G^.  +  Ud^i  I  -  sin(2^  ^  ))) 
e^Ct)  =  95^  -  (495(1  -  Y  +  ^  sin(2TT  i  ))) 

where  T  =  the  total  time  found  in  Section  2.7 

t  =  the  value  for  time  using  the  time 
incremen.  4t  . 

The  two  values  of  orientation  angles  ^5^,65  ,  are  then  converted  to 

wrist  motor  (A  and  B)  angles  >  using  the  transformations  shown  at 

the  end  of  Section  2.3. 

2.9  Simulation 

An  algorithm  implementing  this  straight  line  path  program  was 
written  in  C  and  simulated  on  a  VAX  11/782.  The  initial  values  of  the 
joint  angles  and  the  final  values  of  point  A  and  orientation  angles  were 
entered  from  the  keyboard.  The  program  follows  this  outline: 

1)  Read  the  initial  joint  angles  (shoulder  yaw,  iy 

shoulder  pitch,  ,  elbow,  ,  wrist  motor,  ,  and 
wrist  motor,  v^)  from  the  keyboard  (all  values  were 
entered  in  radians). 
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2)  Convert  joint  angles  {9-|, 62,62^1  cylindrical  coordinates 
(0,R,Z).  for  the  initial  position, 

3)  Convert  the  wrist  motor  angles  to  wrist  orientation 

angles  (wrist  yaw,  0^,  and  wrist  roll,  9^)  for  the  initial 
position. 

4)  Enter  the  final  position  of  point  A  in  cylindrical 
coordinates  (9,R,Z)^  and  the  final  orientation  angles 
(9^^,02^)  .  All  values  are  entered  in  meters  and  radians. 

5)  Compute  constants  (A9,itR,AZ,  the  initial  and  final 
position  of  point  A  in  the  X,Y^  ,  plane,  the  equation  for 
the  straight  line  between  the  two  points). 

6)  Using  jAe^l  (the  change  in  shoulder  yaw,  which  is 
also  A0  in  cylindrical  coordinates),  compute  T  (the 
total  time  to  traverse  the  straight  line  path). 

7)  Start  a  loop  (from  t=0  until  t=T,  incrementing  t  by  At 
(where  At  used  in  simulation  was  .25  seconds  instead  of 
the  sampling  time  (-.05  seconds),  this  was  done  for  the 
sake  of  brevity  during  simulation). 

a)  Compute  0i(t),  the  driving  function,  for 
the  appropriate  case  as  in  Section  2.7. 

b)  Compute  R  and  Z  for  point  A  at  that 
particular  9  (found  in  step  7a)  where  0  ,R  and 
Z  are  constrained  by  the  straight  line  equation 
obtained  in  step  5. 
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c)  Compute  the  three  joint  angles  (shoulder  yaw,  0^ 
shoulder  pitch,  0^  ,  and  elbow,  e^)  frorr  the 
cylindrical  coordinates  (0,R,Z)  found  in  step  7b. 

d)  Calculate  the  wrist  yaw,  9^  ,  and  the  wrist  roll, 

,  using  the  appropriate  driving  functions, 
9^(t)  and  O^Ct). 

e)  Check  the  limits  for  all  joint  and  orientation 
angles  (9^ ,02,92, 9^, 6^) . 

f)  Convert  wrist  orientation  angles  (0^,02)  to  wrist 
motor  angles 

g)  Compute  the  velocity  and  acceleration  of  the 
shoulder  yaw  using  the  equations  in  Section  2.7. 

h)  Plot  the  following  graphs: 

-shoulder  yaw  versus  time 
-shoulder  yaw  velocity  versus  time 
-shoulder  yaw  acceleration  versus  time 
-wrist  yaw  versus  time 

-wrist  roll  versus  time 
-the  position  of  point  A  in  X,Y,Z  space 
-X  versus  Y 
-X  versus  Z 


-Y  versus  Z  . 
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Many  simulations  were  performed  using  this  algorithm  with  excellent  results. 
The  plots  show  how  gradual  acceleration  and  braking  were  applied  to 
shoulder  yaw,  0^  ,  wrist  yaw,  9^  ,  and  wrist  roll,  9^  .  The  plots  of 
point  A  in  the  X,Y,Z  plane  show  how  the  path  of  point  A  is  a  straight  line. 
Three  of  these  simulation  results  follow. 

Simulation  1)  -Shoulder  yaw  initial,  ,  <  shoulder  yaw 
final ,  9i^ 

-the  elbow  is  in  the  elbow  down  position 
(02  (See  Figure  2.20.) 


FIGURE  2.20  Arm  Position  Simulation  I 


SMOULDER  YAW  VELOCITY 
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time  (seconds) 

FIGURE  2.2]  Shoulder  Yaw  Position  -  Simulation  I 


0  0  ;  .0  1  0.0  0.5  .0  0,5  4.3  4 
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FIOiJDE  2.22  Shoulder  Yaw  Velocity  -  Simulation  I 


WRIST  YAW  (THtTA  4)  SHOULDER  YAW  ACCELERATION 
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C  0  0  5  i  .  0  1.5  2.0  2,5  3,0  3.5  4.0  4.5  5.0 


TIME  (seconds) 

IGURE  2.23  Shoulder  Yaw  Acceleration  -  Simulation  I 


0  0  0  1.0  ’  . 2.0  ,0.2.  50  3  5  4  0  4 


TIME  (seconds) 

FIGURE  2.24  Wrist  Yaw  Position  -  Simulation  I 


WRIST  ROLL  (THETA  5) 
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e  0  0.5  1,0  1.5  2.0  2.5  3.0  3.5  4,0  4.5'  ,  O 


TIME  (seconds) 

FIGURE  2.25  Wrist  Roll  Position  -  Simulation  I 


FIGURE  2.26  i  vs.  .<  Position  of  Pt.  A  -  Simulation  I 
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0.40 


FIGURE  2.27  Z  vs.  X  Position  of  Point  A 


0.40  - 

i 


^  t 


U  .  it' 


FIGURE  2.28  Z  vs.  Y  Postion  of  Point  A 


-  Simulation  1 


O'  . 


-  Simulation  I 
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FIGURE  2.30  Shoulder  Yaw  Position  -  Simulation  II 


TIME  (seconds) 

F'.'jUPE  2.31  Shoulde''’  Yaw  Velocity  -  Simulation  II 


SHOULDER  YAW  ACCELERATION 
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FIGURE  2.32  Shoulder  Yaw  Acceleration  -  Simulation  II 


TIME  (seconds) 
Wrist  Yaw  Position  - 
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Simulation  II 


WRIST  ROLL  (THETA  5) 
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0  0  0.0  1.0  1.5  2.0  2  3.0  3.5  4.0  4,5 


TIME  (seconds) 

FIGURE  2.34  Wrist  Roll  Position  -  Simulation  II 


FIGURE  2.35  Y  vs.  i  Position  of  Pt.  A  -  Simulation  II 
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-  increment  the  time  step 

-  synchronize  the  control  system 

-  start  again  at  the  beginning  of  the  loop 
until  the  total  time  is  reached. 

hen  the  final  point  is  reached,  the  function  returns  control  to  the  call- 
ng  function.  The  calling  function  in  this  case  is  the  option  block, 
he  option  used  to  select  this  method  of  control  is  discussed  next. 

Using  screen  functions  supplied  by  the  compiler,  the  operator  can 
elect  many  different  options  by  using  a  character  on  the  keyboard.  These 
iptions  are  outlined  in  Appendix  A  (Sect.  3,  Part  0.5).  A  new  option  was 
idded  to  the  option  block;  if  the  operator 'selects  'm' ,  the  right  arm 
;ill  I'lOve  the  point  A  to  the  specified  final  point,  point  B,  and  the 
Mnal  orientation  angles.  The  new  option  is  outlined  below. 

-  select  the  supervisory  mode 

-  select  option  'm' 

-  the  master  arm  is  read  to  find  the  starting  configuration 
of  the  arm 

-  the  final  coordinates  of  point  A  and  the  final  orientation 
angles  are  recalled  from  memory 

-  the  move()  function  is  called.  The  function  is  passed 
the  initial  angular  configuration  of  the  arm  in  integers 

and  the  values  for  the  final  cylindrical  coordinates  of  point 
A  and  the  final  orientation  angles. 
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convert  the  integers  of  each  angle 

the  initial  position  to  the  appropriate  radian  value. 

convert  the  initial  position  of  the  arm  to  cylindrical 

coordinates  (6,R,Z)  of  point  A  and  to  wrist  orientation 

angle  [it  already  has  the  final  cylindrical 

coordinates  of  point  A  and  the  final  orientation 

angles 

compute  the  constants  necessary  for  the  straight 
1 i ne  path  constraint 
start  the  loop 

-  using  the  driving  function  compute  the  cylindical 
coordinates  of  point  A  on  the  straight  line  path 
for  that  time  step 

-  using  the  driving  functions  for  wrist  yaw  and 
roll,  compute  the  wrist  yaw  and  roll  angles 

for  that  time  step 

-  convert  the  cylindrical  coordinates  of  the 

point  A  to  joint  angles  (shoulder  yaw, 

shoulder  pitch  and  elbow) 


-  check  each  angle  for  their  limits 

-  convert  the  orientation  angles  (  to  wrist 

motor  angles  (14, V'^) 

-  convert  the  angles  from  radians  to  integers 


-  send  the  integers  for  each  angle  (^i’^2’  3 


■'4 


to  the  control  system 
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Wrist  Motor  B, 


Since  the  relationship  between  the  angular  position  and  the  integer  output 
of  the  sensor  (a  linear  resistance  potentiometer)  is  a  linear  one,  the 
conversion  from  integer  to  radian  is  a  simple  equation.  The  data  from 
the  diagrams  shown  was  used  to  develop  the  equations  for  each  angle; 

Tt  > Vo.Tojyyi  vr'c  •  Therefore,  in  this  step  the  central  controller  uses 

I  c  J  4  b 

the  existing  functions  in  the  BAT  software  (Appendix  A,  Sect.  3)  to  read 
the  master  arm.  It  takes  these  integer  values  and  converts  them  to 
radian  values  using  the  equations  developed  from  the  data  shown  in  the 
diagrams  above.  The  central  controller  now  knows  the  configuration  of 
the  master  arm  in  radian  measurement  of  8^  and  .  Here 

again  the  assumption  is  made  that  the  configuration  of  the  master  arm  is 
the  same  as  the  configuration  of  the  BAT's  right  arm.  These  equations  are 
easily  inverted,  so  that  they  are  used  to  convert  from  radian  measurement 
to  integer  value.  This  is  used  later  in  the  procedure. 

Now,  the  central  controller  knows  the  initial  position  of  the 
arm  and  the  final  point.  For  this  option  it  calls  the  straight  line 
path  generation  function,  now  called  move().  The  move{)  function  is  a 
streamlined  version  of  the  simulation  program  used  in  the  last  chapter. 

The  moveO  function  is  outlined  below. 
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The  procedure  for  obtaining  the  wrist  motor  integers  was  a  bit  different. 
Knowing  the  limits  of  wrist  yaw  and  wrist  roll,  the  master  arm's  wrist 
was  moved  to  these  limits  and  the  integers  for  the  wrist  motors  A  and  B 
were  recorded. 

6.  =  0.0  radians 


9,  =  3.14  radians 

MAX 


=-1.74  radians 

^MIN 

~r  =1.74  radians 
^MAX 

Using  the  conversion  from  reference  [3]  (also  in  Section  2.3),  the 
corresponding  limits  for  the  wrist  motors  A  and  B  were  found.  The 
data  for  these  motors  is  shown  below. 

880  H 


-4.7657 
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Wrist  Motor  A, 
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The  second  step  in  the  procedure  is  to  read  the  angles  of  the 
master  arm.  Referring  to  Figure  2.1,  the  feedback  from  the  right  arm 
on  the  BAT  does  not  come  back  to  the  central  controller;  the  feedback 
stops  at  the  joint/actuator  cards.  Therefore,  the  master  arm  positions 
are  used  to  let  the  central  controller  know  where  the  right  arm  of  the 
BAT  is.  This  is  also  covered  in  Appendix  A.  Reading  the  master  arm 
lets  the  central  controller  know  the  initial  angles  of  shoulder  yaw, 
shoulder  pitch,  elbow,  wrist  motor  A  and  wrist  motor  B.  Discussed  in 
Appendix  A,  Sect.  3,  Part  D.3,  the  program  uses  three  functions  to  read 
and  adjust  the  analog  inputs  from  the  ICS.  The  five  master  arm  potentio¬ 
meters  are  read  along  with  the  camera  tilt  and  pan  and  the  left  arm  roller 
motor.  In  this  case,  the  program  will  read  only  the  five  master  arm 
potentiometers.  These  functions  will  return  integer  values  for 
-j-j  ,02  and  .  The  integers  come  from  an  adjusted  analog  to 

digital  conversion  and  represent  12-bit  two's  complement  binary  numbers. 
Using  one  of  the  options  described  in  Appendix  A,  Sect.  3,  Part  D.5,  the 
software  was  run  with  all  power  to  the  BAT  shut-off.  An  option  was  used 
to  show  the  hexadecimal  number  read  by  the  central  controller  when  the 
master  arm  was  moved  through  each  of  t'- e  joint's  limits.  This  hexadeci¬ 
mal  number  represented  the  integer  from  the  adjusted  analog  to  digital 
conversion.  For  example,  the  maste’^  arm's  shoulder  yaw  was  moved  from 

■'I  to  and  the  corresponding  numbers  were  recorded.  The  three 

:’’IN  'max 

joint  angle  (shoulder  yaw,  shoulder  pitch  and  elbow)  measurements  in 


radians  and  hexadecimal  numbers  are  shown  below. 
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in  this  figure,  the  operator  has  grasped  two  structural  components 
with  each  arm  and  would  like  to  make  a  connection.  A  supervisory  option 
is  selected  and  the  right  arm  moves  autonomously  to  point  B.  Point  B  was 
chosen  to  be  next  to  the  female  end  of  the  left  arm's  beam.  The  operator 
then  switches  back  to  master-slave  control  to  make  the  connection.  Point 
B  was  measured  in  cylindrical  coordinates,  in  reference  frame  Xq,Yq,Zq 
(Section  2.3),  and  has  the  following  values: 

V  =  2.26  radians  (also  this  is  shoulder  yaw) 

R  =  .6  meters 

Z  =  -.1  meters 

The  final  orientation  angles  (■:‘4,;j)  were  chosen  to  be: 

wrist  yaw,  =  1.57  radians 

wrist  roll,  =  0.0  radians 

These  values  are  stored  as  constants  in  an  array  at  the  beginning  of  the 
option.  A  new  position  can  be  measured  and  the  values  stored  any  time 
prior  to  the  start-up  procedure.  In  the  future,  an  input  function  could 
be  added  to  this  option,  so  that  the  final  point  can  be  changed  on-line. 
Presently,  to  speed  up  experimentation  with  this  method  of  control,  the 
final  point  is  stored  as  a  constant  in  the  software. 
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b)  At  each  time  step,  the  radian  measurement  for 
each  angle  in  step  4a)  is  converted  to  the  appropriate 
integers  for  the  control  system. 

c)  At  each  time  step,  the  integers  are  sent  to 
the  control  system. 

5)  Control  is  returned  to  the  main  program. 

The  final  position  was  chosen  to  be  point  B,  shown  in  Figure  3.1. 


FIGURE  3.1  Finish  Point  -  Implementation  I 
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CHAPTER  3 
IMPLEMENTATION  I: 

UNKNOWN  STARTING  POSITION,  KNOWN  FINISH  POINT 

Using  the  straight  line  path  generation  program  developed  in  the 
last  chapter,  supervisory  control  of  the  BAT's  right  arm  can  be  added  to 
the  BAT  software.  With  a  specified  finish  point,  the  operator  can 
select  an  option  using  the  keyboard  and  the  right  arm  will  move  to  that 
point.  The  procedure  is  outlined  below. 

1)  The  final  point  A  is  stored  in  the  program  in 
cylindrical  coordinates  (■•,R,Z)^  along  with  the  final  wrist 
orientation  angles  (  .  ,•.>  ). 

4f 

2)  The  shoulder  yaw,  shoulder  pitch  and  elbow  angles 

^■l’'2’"3^i  wrist  motor  A  and  B  angles 

(v4>V5)^  read  from  the  master  arm  potentiometers. 

3)  The  angles  read  in  step  2  are  in  integers  and 
must  be  converted  to  the  appropriate  angular  measurement 
(radians) . 

4)  The  straight  line  path  generation  program  is 
called. 

a)  At  each  time  step,  the  program  generates 
shoulder  yaw,  0^  ,  shoulder  pitch,  0^  ,  elbow,  6^  , 
wrist  motor  A,  ,  and  wrist  motor  B,  angles 

in  radians  keeping  point  A  on  a  straight  line  path. 


-70- 


This  program  will  be  used  to  implement  the  supervisory  control  of 
the  right  arm.  The  first  trajectory  will  be  from  an  unknown  starting 
position  to  a  known  finish  point  and  will  be  discussed  in  the  next 
chapter. 


TIME  (seconds) 

FIGURE  2.43  Wrist  Roll  Position  -  Simulation  III 


FIGURE  2.44  Y  vs.  X  Position  of  Point  A  -  Simulation  III 
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FIGURE  2.41  Anti  Radius  Acceleration  -  Simulation 
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FIGURE  2.42  Wrist  Yaw  Position  -  Simulation  III 
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FIGURE  2.39  Shoulder  Yaw  Position  -  Simulation  III 
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FIGURE  2.40  Arm  Radius  Velocity  -  Simulation  III 


-  after  the  arm  has  completed  the  move,  exit  the 
supervisory  mode. 

The  space  bar  is  used  as  an  emergency  exit  from  both  thi^  option  and 
the  supervisory  mode. 

The  program  was  tested  in  two  steps: 

step  1)  select  the  option,  enter  the  initial  arm 
configuration  in  integers  instead  of  using  the 
master  arm,  write  the  joint  angles  (6^ 
to  a  file  instead  of  sending  them  to  the  control 
system.  This  step  checks  the  program  flow  and  the 
integer/radian  conversions. 

In  step  one,  the  following  integers  (shown  with  their  corresponding 
radian  value)  were  entered: 

Shoulder  yaw,  f-j  =  .785  corresponds  to  3373. 

Shoulder  pitch,  =  1*57  radians  corresponds  to  110 

Elbow,  =  .785  radians  corresponds  to  3130 

Wrist  yaw,  =  0.0  radians 

Wrist  roll,  =  1,57  radians 

these  correspond  to: 

Wrist  motor  A,  =  -1.46  radians  corresponds  to  4064 

Wrist  motor  B,  =  -1.46  radians  corresponds  to  2550. 


Shoulder  Yaw  (Theta  1 ) 


Y  Pos.  of  Pt.  A  (meters) 


FIGURE  3.8 


Z  vs.  Y  Position,  Step  1,  Implementation  I 


As  shown  by  the  plots,  there  is  a  gradual  acceleration  and  gradual 
braking  of  the  shoulder  yaw,  wrist  yaw  and  wrist  roll  angles.  The  last 
three  plots  show  how  the  program  does  generate  a  straight  line  path  for 
point  A  and  it  does  finish  ac  point  B. 

Step  2)  Select  the  option,  send  the  joint  angles 

^^1 ’^2 ’“3 ’"4 ’^5^  to  a  file  instead  of  to  the 
control  system.  This  step  checks  everything 
except  the  actual  movement  of  the  BAT's  right 
arm. 

This  step  was  run  at  the  ICS  and  the  values  obtained  are  shown  in  the 
plots  which  follow. 


TIME  (seconds) 

FIGURE  3.9  Shoulder  Yaw  Position,  Step  2,  Implementation  I 
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Y  Pos.  of  Pt.  A  (meters) 


FIGURE  3.14  Z  vs.  Y  Position,  Step  2,  Implementation  II 


The  plots  show  how  the  angles  (shoulder  yaw,  wrist  yaw,  and  wrist  roll) 
are  gradually  accelerated  and  gradually  braked.  The  last  three  plots 
demonstrate  the  straight  line  generation  of  point  A. 

Actual  tests  done  on  the  BAT  with  this  method  of  control  will 
be  dis'.ussed  in  the  last  chapter.  This  chapter  shows  that  the  right 
arm  control  system  can  be  upgraded  to  supervisory  control.  This  now 
gives  the  operator  the  capability  of  moving  the  right  arm  to  a  specified 
position  without  using  the  master  arm. 
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CHAPTER  4 
IMPLEMENTATION  II: 

LEARNING  TRAJECTORIES  BY  CONCATENATION 

The  last  chapter  demonstrated  that  the  central  controller  could 
control  the  right  arm  of  the  BAT,  by  moving  the  right  arm's  end 
effector  in  a  straight  line  path  from  any  point  to  any  other  point, 
within  the  arm's  range  and  flexibility  limits.  An  improvement  on  this 
capability  would  be  to  enable  the  central  controller  to  remember  a 
sequence  of  points,  concatenate  their  paths  and  move  the  right  arm  along 
that  specified  trajectory.  This  can  be  done  easily  with  the  move() 
function  developed  in  the  last  chapter  and  the  addition  of  three  new 
options  in  the  software. 

The  first  new  option  enables  the  central  controller  to  remember 
a  sequence  of  points  in  space.  These  are  the  points  that  the  operator 
would  like  the  path  of  the  right  arm's  end  effector  to  move  through.  It 
can  be  done  by  reading  the  position  of  the  master  arm  and  storing  that 
position  in  an  array.  This  operation  consists  of  four  steps. 

1)  enter  the  supervisory  mode 

2)  choose  this  option  by  hitting  the  'x'  on  the 
keyboard 

3)  the  central  controller  reads  the  five  potentiometers 
of  the  master  arm  corresponding  to  the  shoulder  yaw,  0.|  , 
shoulder  pitch,  ,  elbow,  »  wrist  motor  A,  ,  and 
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wn'st  motor  B,  .  It  stores  these  integers  in  a  two- 
dimensional  array  (7  rows  by  5  columns)  with  each  column 
corresponding  to  ,62,62 (the  integer  value)  in  that 
order.  The  entire  first  row  is  skipped.  This  row  is  saved 
for  the  starting  point.  An  operator  can  store  as  many  as 
five  points.  The  last  row  is  saved  for  the  finish  point 
which  is  the  same  point  as  the  starting  point.  Obviously, 
each  row  is  a  right  arm  position. 

4)  return  to  the  calling  function. 

The  maximum  number  of  points  stored  can  be  increased  easily  by  making 
the  array  have  more  rows.  For  this  thesis  it  will  remain  at  five. 

The  second  new  option  allows  the  operator  to  clear  the  array  of 
all  points  and  start  over.  When  the  'y'  key  is  selected,  the  option 
simply  re-initial izes  the  array's  elements  to  all  zeroes.  It  then 
returns  to  the  calling  function. 

Now  the  operator  has  the  capability  of  storing  any  points  in 
the  right  arm's  range  of  motion.  The  operator  simply  moves  the  right 
ir- .  the  master  arm,  to  a  point  and  selects  'x'.  The  point  is 

*  "‘I  I.  1  row  of  integers  corresponding  to  the  master  arm's  potentio- 
■  of  the  joint  angles.  The  operator  then  goes  to  the  next 

o  ;  .■  ^■ots  'x'  again.  This  point  is  then  stored  in  the  row  below 
t'-  *  '  •  *  rei  point.  The  operator  does  this  until  all  the  points 

Of  ,  1 ' .  .1  stored.  The  software  joins  these  points  in  sequence  to 
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tnake  a  desired  trajectory.  (See  Figure  4.1.)  The  operator  has  the 
capability  of  Clearing  the  trajectory  and  starting  over.  This  is  used 
when  entering  another  trajectory  or  when  a  mistake  has  been  made  in 
entering  one  of  the  points. 


FIGURE  4.1  Implementation  II 

The  final  option  is  the  one  which  will  move  the  right  arm  from 
wherever  it  is,  through  the  sequence  of  points  which  were  stored  and 
then  move  back  to  where  it  originally  started.  The  paths  through  all 
these  points  will  be  a  straight  line.  The  outlines  of  this  new  option 
is  1 isted  below. 
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1)  enter  the  supervisory  mode 

2)  choose  to  move  through  all  the  stored  points  by 
selecting  'z'  on  the  keyboard 

3)  read  the  current  position  of  the  master  arm  and 
store  this  position  in  the  array  as  the  first  row  and 
directly  after  the  last  row  stored  (for  example,  if  only 
three  points  have  been  stored  when  'z'  is  selected  the 
current  position  is  in  row  number  1  and  row  number  5,  row 
numbers  2,3,4  are  the  three  points  already  stored  by 
selecting  'x') 

4)  start  a  loop  in  which  the  program  takes  two  rows 
of  integers  at  a  time,  starting  with  the  first  and  second 
row,  and  set  these  points  (a  row  of  integers)  equal  to  the 
initial  and  final  positions  for  the  straight  line  path 

5)  convert  the  integers  of  the  final  position  to 

cylindrical  coordinates  of  point  A  and  final  wrist 
orientation  angles  (■:■  .  Of  course,  to  do  this 

it  must  first  convert  the  five  integers  to  shoulder  yaw 

and  pitch,  elbow,  and  wrist  motor  angles  to  radians.  It  then 
uses  the  radian  values  of  -■]  >•^2  "^3  convert  to 

^•,R,Z  of  point  A  and  to  convert  to  .  This  was 

covered  in  Chapter  2. 

6)  call  the  move()  function  passing  it  to  the  initial 
position,  which  is  the  first  rcw  of  angles  in  integers,  and 
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the  final  point  A  and  orientation  angles,  found  in  step  5. 
the  move()  function  moves  the  end  effector  of  the  right 
arm  (point  A)  from  the  initial  position  to  the  final  point 
in  a  straight  1 ine  path . 

7)  when  the  move()  function  is  completed,  control 
goes  back  to  step  4  and  takes  the  next  set  of  two  points 
(in  this  case  rows  two  and  three,  the  next  time  it  will 
take  rows  three  and  four  and  so  on)  and  repeats  steps  5,  6 
and  7 

8)  when  the  last  pair  is  used  the  right  ar'-.  will  be 
back  at  the  starting  point  (this  is  because  the  cu>"rent 
position  was  also  stored  as  the  last  point  in  the  array) 

9)  control  is  returned  to  the  calling  function. 

In  testing  this  method  of  concatenating  points  the  same  two 
;ep  procedure  used  in  testing  the  method  of  Chapter  3  was  used  here, 
rst,  all  entries  were  made  as  integers  by  keyboard  input  and  all 
igles  were  sent  to  a  file.  This  tested  program  flow  and  integer/radian 
inversion.  Second,  the  master  arm  was  used  to  enter  the  points  and 
le  values  were  sent  to  a  file.  This  tested  all  aspects  of  the  program 
:cept  the  actual  movement  of  the  BAT's  right  arm.  Actual  tests  done 
1  the  BAT  will  be  discussed  in  Chapter  5. 
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step  1)  Keyboard  entry,  values  sent  to  a  file  (see  Figure  4.2). 


Shoulder  yaw 
Shoulder  pitch 
Elbow 
Wrist  yaw 
Wrist  roll 

Wrist  motor  A 


START/FINISH 
.785  ra^,,---'''^ 
3373 

1  .570 

no 

.785  racL  " 

-  -  3130 

1.570 

0.0  rad 

-1.57  rad-.-'^’^ 


POINT  1 


POINT  2 


1.5707  ra^^'^'"^ 
4090 

1.570  rad.^-^ 
4090 

1.91986  rad  ^ 

3662 

1.919  rad,,/"'' 

'  3662 

.78539  rad^" 
3130 

1.57  rad  / 

3894 

0.0  rad 

.785  rad 

1.570  rad 


.785  rad 


POINT  3 
1.22  rai- 


2.18  rad 


1.047  rad 


2.35  rad 


0.0  rad 


1.57  racL--^  i  -1.462  ra^^^  -1.516  rad^^'  -2.35  rad 

4002  i  4064:  .  4033  _ ^53 

1.570  rad  '  -1.462  .054  rad  ^  2.35  rad 


Wrist  motor  B 
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Shoulder  Yaw  Position,  Step  1,  Implementation  II 
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4,5  Wrist  Roll  Position,  Step  1,  Implementation  II 
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FIGURE  4.6  Y  vs.  X  Position,  Step  1,  Implementation  II 


Z  Pos.  of  Pt.  A  (meters) 
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X  Pos.  of  Pt.  A  (meters) 

FIGURE  4.7  Z  vs.  X  Position,  Step  1,  Implementation  II 


The  plots  show  the  gradual  acceleration  and  braking  applied  to  the  shoulder 
yaw,  wrist  yaw  and  wrist  roll  as  the  arm  moves  through  all  the  points.  The 
plots  in  X,Y,Z  space  show  how  the  arm’s  end  effector  position  (Point  A)  is 
kept  on  a  straight  line  path  through  all  the  points.  It  also  shows  how  the 
arm  moves  in  a  closed  loop.  It  starts  and  finishes  at  the  same  point. 

Step  2)  Master  arm  entry,  all  values  sent  to  a  file. 


TIME  (seconds) 

FIGURE  4.9  Shoulder  Yaw  Position,  Step  2,  Implementation  II 
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FIGURE  4.10  Wrist  Yaw  Position,  Step  2,  Implementation  II 
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FIGURE  4.11  Wrist  Roll  Position,  Step  2,  Implementation 


Y  Pos.  of  Pt.  A  (meters) 
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FIGURE  4.12 


X  Pos.  of  Pt.  A  (meters) 

Y  VS.  X  Position,  Step  2,  Implementation  II 


X  Pos.  of  Pt.  A  (meters) 

figure  4.13  Z  vs.  X  Position,  Step  2,  Implementation  II 
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FIGURE  4.14  Z  vs.  Y  Position,  Step  2,  Implementation  I 


Here  again,  the  plots  give  the  same  type  of  results;  the  gradual  accelera¬ 
tion  and  braking,  the  straight  line  paths  and  the  closed  loop  trajectory. 

The  operator  now  has  a  great  deal  of  supervisory  control  over  the 
right  arm.  The  only  time  the  operator  needs  to  use  the  master  arm  is  to 
store  points  for  a  trajectory.  He/she  could  then  put  the  master  arm  in 
a  set  position  and  use  the  'z'  option  to  perform  a  movement  task  over  and 
over.  This  will  help  free  the  operator's  hands  for  other  functions  at 
the  ICS.  The  extensions  of  this  will  be  discussed  in  the  next  chapter. 
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CHAPTER  5 
CONCLUSIONS 

The  two  implementations  discussed  in  Chapters  3  and  4  were  in¬ 
corporated  into  the  existing  BAT  software.  The  operator  can  enter  the 
supervisory  control  mode  by  either  hitting  the  'w'  key  or  by  pushing  the 
black  pushbutton  between  the  joysticks  on  the  ICS's  bottom  control  panel. 

The  code  for  the  supervisory  control  mode  is  listed  in  Appendix  B.  While 
the  BAT  was  in  the  laboratory  (not  in  the  neutral  buoyancy  environment), 
the  software  was  tested.  All  of  the  BAT’s  right  arm  actuators  were 
powered  up  except  the  shoulder  yaw  motor.  It  would  place  too  much  strain 
on  the  motor  if  it  was  driving  the  arm  outside  the  neutral  buoyancy 
environment.  At  the  ICS  the  supervisory  mode  was  selected  and  both  imple¬ 
mentations  (unknown  starting  position  to  a  known  finish  point;  concatenating 
points)  were  run.  Since  the  shoulder  yaw  motor  was  not  powered  up  it  could 
not  be  ascertained  as  to  whether  the  path  of  point  A  (the  end  effector)  was 
a  straight  line  path.  It  was  very  apparent  that  the  arm  movement  was  not 
erratic  and  was  very  smooth.  The  joints  that  were  powered  did  move  with 
gradual  acceleration  and  braking.  When  the  second  implementation  was  per¬ 
formed  the  arm  did  pass  through  all  the  points  and  returned  to  the  finish 
point. 

The  supervisory  control  of  the  BAT  was  tested  in  the  neutral 
buoyancy  environment  by  the  Space  Systems  Laboratory.  While  in  master- 
slave  control,  one  of  the  wrist  motors  was  running  intermittently.  This 
caused  a  sudden  jerk  in  the  movement  of  the  wrist  since  the  two  wrist 
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motors  are  coupled  to  give  wrist  yaw  and  roll.  In  the  supervisory  mode, 
when  both  implementations  were  tested,  the  same  jerks  also  occurred  in  the 
wrist  movement.  Otherwise,  the  wrist  moved  to  the  points  with  a  gradual 
acceleration  and  a  gradual  braking.  The  elbow  moved  smoothly  throughout 
the  test  in  both  master-slave  and  supervisory  mode.  Shoulder  yaw  and 
pitch  operated  smoothly  in  master-slave  control,  but  did  not  move  at  all 
when  the  supervisory  options  were  selected.  Due  to  an  oversight  in  the 
development  of  the  software,  the  shoulder  pitch  and  yaw  commands  were 
not  being  sent  to  the  control  system.  This  was  easily  corrected  but  not 
in  time  to  be  tested  again.  Since  only  the  elbow  and  wrist  were  being 
controlled,  the  end  effector  did  not  move  in  a  straight  line  path. 

Although  the  results  of  the  test  are  inconclusive  as  to  whether  the 
supervisory  control  method  will  move  the  arm  in  a  straight  line  path  to  a 
specified  point,  it  does  show  that,  in  the  neutral  buoyancy  environment, 
the  elbow  joint  was  controlled  smoothly  and  gradually.  The  next  test  will 
be  directed  towards  insuring  all  the  joints  are  controlled  properly.  Upon 
completion  of  this  step,  the  accuracy  of  the  joint  positions  will  be 
evaluated. 

From  the  plots  of  the  simulations  in  Chapter  2  and  the  plots  of  the 
implementations  in  Chapters  3  and  4,  it  is  easy  to  see  that  for  the  given 
inputs  the  arm  will  move  in  a  straight  line  path.  With  the  test  on  the 
BAT  in  the  laboratory,  the  results  showed  that  the  program  flow  was  working 
properly  and  the  arm  was  moving  smoothly  with  gradual  acceleration  and 
braking.  When  the  program  is  tested  on  the  BAT  again  in  the  neutral 
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buoyancy  environment,  the  only  cause  for  error  is  if  the  relationship 
between  the  BAT's  right  arm  joint  angles  and  the  joint  angles  which  the 
central  controller  generates  is  not  the  same.  This  refers  back  to  the 
assumption  that  the  master  arm's  position  gives  the  correct  position  of 
the  bat's  right  arm.  This  can  be  corrected  by  simply  readjusting  the 
integer/radian  conversion  between  the  master  arm's  analog  to  digital 
conversion  and  the  angular  position  of  the  BAT's  right  arm  joints.  Once 
the  integer/radian  conversion  is  accurate,  the  previous  results  conclude 
that  the  arm's  end  effector  will  move  in  a  straight  line  path  smoothly  whil 
using  this  method  of  supervisory  control. 

With  this  method  of  controlling  the  BAT's  right  arm  movements,  many 
extensions  can  be  incorporated  into  the  BAT's  software,  so  that  the  control 
relationship  will  be  almost  entirely  supervisory.  Other  functions  can  be 
added  to  the  software,  so  that  the  right  arm  can  perform  many  other  tasks 
by  using  keyboard  entry  at  the  central  controller.  For  example,  a  function 
called  rotate  (arguments)  can  be  developed.  Here  the  orientation  angles 
(wrist  yaw  and  wrist  roll)  are  changed  according  to  the  arguments  of  the 
function  (i.e.,  rotate  ("t/2,  0)  would  change  the  wrist  yaw  7i/2  radians 
and  the  wrist  roll  0  radians).  There  could  also  be  other  functions  like 
lift  (arguments)  where  just  the  shoulder  pitch  and  elbow  angles  would  be 
changed,  so  that  the  arm's  end  effector  would  move  in  a  straight  vertical 
line.  All  these  functions  can  be  developed  and  stored  in  a  library.  The 
operator  could  then  perform  complete  assembly  tasks  by  simply  typing  in  a 
list  of  these  functions  for  the  BAT  to  follow.  For  example,  a  list  could 
be  as  follows; 
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small  motor  drive  circuit  can  handle  only  50  amps.  This  provides  all 
eight  drives  (the  camera  tilt  and  pan,  the  five  right  arm  motors  and 

the  left  arm  motor)  necessary  for  the  operation  of  the  BAT.  The  control 

of  the  right  arm  is  discussed  in  more  detail  in  Appendix  A,  Section  3. 

D)  Propulsion  Subsystem  (see  Figure  A. 8) 

There  are  eight  electric  trolling  motors  pressurized  for  water¬ 
proofing.  They  combine  the  thrust  to  give  the  BAT  six  degrees-of-freedom. 
The  main  power  subsystem  supplies  power  to  the  power  transistor  modules 
which  drive  the  motors.  The  modules  contain  pulse-width  modulation  drives 
for  each  of  the  eight  motors.  They  are  controlled  from  the  control  box 
propulsion  controller  through  the  relay  box.  The  propulsion  controller 
receives  its  commands  from  the  ICS  through  the  RRT.  An  inertial  package 
provides  attitude  and  rate  feedback  through  the  propulsion  controller  and 
the  RRT  to  the  ICS. 

Aside  from  the  video  signals  from  the  two  cameras  on  the  BAT,  all 
of  the  BAT'S  input  and  output  signals  are  passed  through  the  RRT  in  the 
control  box  to  the  ICS.  All  the  BAT's  input  signals  originate  at  the  ICS, 
and  all  the  BAT's  output  is  fed  back  to  the  ICS. 

A. 2  Hardware  of  the  ICS 

During  the  neutral  buoyancy  tests  the  BAT  is  controlled  by  the 
Integrated  Control  Station  (ICS) (see  Figure  A. 9).  The  ICS's  frame  is 
made  of  welded  angle  stock  and  mounted  on  wheels  so  it  can  be  transported 
to  test  sites  with  ease.  It  has  room  to  hold  the  operator,  the  controls 
and  their  displays,  and  video  equipment. 
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FIGURE  A. 7  Right  Arm  Subsystem 
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motors  receive  their  input  commands  from  the  ICS  through  the  RRT  and  two 
power  modules.  These  power  modules  will  be  described  in  further  detail 
in  the  right  arm  subsystem.  Two  optical  encoders  are  used  for  tilt  and 
pan  position  feedback  to  the  interfaces  in  the  control  box.  The  cameras' 
video  signals  are  connected  to  video  hardware  at  the  ICS  through  coax 
cables. 

B)  Left  Arm  Subsystem  (see  Figure  A. 6) 

The  left  arm  is  a  specialized  actuator  which  can  grasp  a  beam  and 
move  it  laterally  across  the  body  of  the  BAT.  It  grasps  the  beam  by  means 
of  a  specialized  end  effector,  which  is  opened  and  closed  pneumatically. 

It  is  controlled  by  the  ICS  through  the  solenoid  valve  box  and  the  RRT.  At 
the  base  of  the  end  effector  is  a  roller.  This  roller  can  move  a  beam 
across  the  BAT.  The  roller  is  driven  by  a  permanent  magnet  DC  motor.  The 
motor  receives  its  power  from  the  main  power  subsystem  through  a  small  power 
module  (described  in  following  section). 

C)  Right  Arm  Subsystem  (see  Figure  A. 7) 

The  right  arm  is  a  five  degree-of-freedom  manipulator  with  a 
grasping  end  effector.  The  end  effector  is  controlled  pneumatically  from 
the  ICS  through  the  RRT.  Each  of  the  five  degrees-of-freedom  are  driven 
by  a  permanent  magnet  DC  motor.  The  motors  are  connected  to  the  power 
modules,  which  are  pulse-width  modulation  amplifiers.  They  are  controlled 
from  the  ICS  through  the  RRT.  There  are  two  power  modules.  Each  power 
module  contains  four  independent  motor  drive  circuits;  two  large  and  two 
small.  A  large  motor  drive  circuit  can  handle  up  to  100  amps,  while  a 
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CONTROL  BOX 


MAIN  POWER 


FIGURE  A. 5  Video  Subsystem 


supplies  low  pressure  air  to  keep  each  box  in  the  BAT  watertight.  The 
other  tank  supplies  high  pressure  air,  which  is  connected  to  the  solenoid 
valve  box.  The  solenoid  valve  box  uses  this  high  pressure  to  control 
three  components: 

-  the  left  arm  end  effector 

-  the  right  arm  end  effector 

-  the  main  power  relay. 

Both  end  effectors  receive  their  open/close  commands  from  the  ICS 
through  the  RRT.  The  Main  Power  Relay  is  an  electronically-controlled 
pneumatically-operated  switch  which  controls  the  power  output  of  the  main 
battery  box.  Its  normal  operating  position  is  closed.  It  may  be  activated 
from  the  ICS  or  from  a  switch  on  the  BAT.  In  the  event  of  a  communications 
failure  or  upon  activation  it  opens,  disconnecting  the  main  power  from  the 
bat's  actuators  and  thrusters. 

II.  Action  Subsystems 

A)  Video  Subsystem  (see  Figure  A. 5) 

The  video  subsystem  has  two  commercial  cameras,  each  housed  in  an 
aluminum  tube  with  a  front  plexiglas  plate.  The  main  camera  is  mounted  on 
a  tilt  and  pan  unit  on  the  top  of  the  BAT.  A  second  camera  is  mounted 
above  the  right  arm  pivot  and  tracks  the  position  of  the  end  effector.  The 
cameras  are  connected  to  a  camera  support  box,  which  powers  the  cameras 
with  two  gel  cell  lead  acid  batteries  hooked  to  a  voltage  regulator.  The 
platform  is  a  tilt  and  pan  unit  with  two  permanent  magnet  DC  motors.  The 
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FIGURE  A. 4  Pneumatic  Subsystem 
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FIGURE  A. 3  Control  Subsystem 
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-  the  power  amplifiers  of  the  right  arm,  left 

arm  and  the  tilt  and  pan  unit  of  the  main  camera; 

-  the  relay  box  of  the  propulsion  subsystem; 

-  the  battery  monitor  card  in  the  control  subsystem. 

B)  Control  Subsystem  (see  Figure  A. 3) 

The  control  subsystem  has  two  components;  the  control  battery  box 
and  the  control  box.  The  control  battery  box  consists  of  12  Vdc,  5  Vdc 
and  ±6  Vdc  power  supplies  which  supply  the  power  for  the  solenoid  valves 
the  control  logic  and  the  communications  link,  respectively.  Inside  the 
control  box  is  a  battery  monitor  card,  two  encoder  interfaces,  a  propul¬ 
sion  controller  and  communications  hardware.  The  battery  monitor  card 
samples  and  digitizes  the  voltage  levels  of  the  main  batteries  and  the 
air  pressure  transducer.  The  two  optical  encoder  interfaces  are  used 
with  each  actuator  in  the  action  subsystem.  They  convert  each  encoder's 
output  and  transmit  the  outputs  to  the  remote  receiver/transmitter  (RRT) 
The  propulsion  controller  receives  information  from  the  inertial  package 
of  the  propulsion  subsystem.  It  also  sends  commands  from  the  RRT  to 
the  propulsion  subsystem.  The  control  box  communicates  with  the  ICS 
through  the  telemetry  uplink  and  downlink.  This  is  implemented  by  the 
RRT  which  combines  the  uplink  multiplexer  with  the  downlink  digital 
demultiplexer.  It  also  contains  the  analog  transceiver. 

C)  Pneumatic  Subsystem  (see  Figure  A. 4) 

The  pneumatic  subsystem  consists  of  two  separate  air  tanks.  One 


-110- 


FIGURE  A. 2  Main  Power  Subsystem 
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The  Beam  Assembly  Teleoperator  (BAT)  (Figure  A.1)  is  a  self- 
contained  free-flying  teleoperator.  It  was  developed  to  be  used  under 
water  in  neutral  buoyancy  structural  assembly  tests.  The  BAT's  flota¬ 
tion  panels  provide  most  of  its  neutral  buoyancy.  It  is  controlled  from 
an  above-water  control  station,  the  ICS  (described  in  Section  A. 3). 

The  BAT  can  be  broken  down  into  four  support  subsystems  and  three  action 
subsystems. 

I  SUPPORT 

A)  Main  Power  Subsystem 

B)  Control  Subsystem 

C)  Pneumatic  Subsystem 

D)  Propulsion  Subsystem 

I.  Support  Subsystems 

A)  Main  Power  Subsystem  (see  Figure  A.2). 

The  main  battery  box  contains  six  battery  packs  connected  in 
parallel.  Each  battery  pack  contains  three  gelled  electrolyte  lead  acid 
cell  batteries  connected  in  series.  Eighteen  volts  at  20  amp-hours  is 
supplied  by  each  battery  pack.  The  use  of  the  on-board  battery  box 
eliminates  the  need  for  a  cumbersome  umbilical  and  cuts  down  on  power 
loss  through  transmission.  The  main  battery  box  is  connected  through  the 
main  power  relay  (described  in  the  pneumatic  subsystem)  to  the  bus-bar. 
The  distribution  bus-bar  connects  the  main  battery  box  to; 


II  ACTION 

A)  Video  Subsystem 

B)  Left  Arm  Subsystem 

C)  Right  Arm  Subsystem 


APPENDIX  A 


DESCRIPTION  OF  THE  BAT 

I 

The  Beam  Assembly  Teleoperator  was  developed  and  is  used  by 
the  Space  Systems  Laboratory  at  M.I.T.  to  experiment  with  the  man- 
I  machine  interface  for  assembly  tasks.  This  appendix  describes  the 

system  in  detail.  The  appendix  is  broken  down  into  three  sections: 
A.l  Description  of  the  BAT 

A. 2  Hardware  of  the  Integrated  Control  Stations  (ICS) 
A. 3  Software  of  the  ICS. 


A.l  Description  of  the  BAT  (see  Figure  A.l) 
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-  move(arguments) 

-  rotate(argutnents) 

-  1 ift(arguments) 

-  move(arguments) 

This  list  of  functions  can  be  stored  as  a  macro,  so  that  the  operator  can 
simply  hit  a  function  key  (F1,F2,  etc.)  or  type  in  the  macro's  name  and 
the  right  arm  of  the  BAT  would  perform  the  listed  tasks.  The  operator  can 
construct  these  lists,  so  that  the  arm  would  be  performing  a  complete 
assembly  task.  All  these  functions  (except  the  end  effector's  graspO 
and  releaseO  functions)  can  be  developed  by  simply  modifying  the  super¬ 
visory  method  developed  in  this  thesis. 

The  use  of  supervisory  control  on  the  BAT's  right  arm  would  free 
the  operator's  hands  from  the  master  arm  and  let  the  operator  concentrate 
on  other  duties  at  the  ICS.  Supervisory  control  will  increase  the 
capabilities  of  the  BAT  and  help  improve  the  interface  between  man  and 
machine  during  assembly  operations. 
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FIGURE  A. 8  Propulsion  Subsystem 
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FIGURE  A. 9  Sketch  of  the  ICS 


The  ICS  consists  of  five  subsystems; 

A)  ICS  Controls  and  Displays 

B)  Central  Controller 

C)  Joint/Actuator  Control  System 

D)  Propulsion  Control  System 

E)  Communication  Link. 

A)  ICS  Control  and  Displays 

The  ICS  controls  and  displays  system  takes  up  most  of  the  area  of  the 


ICS.  It  contains  the  helmet  for  video  control,  the  master  arm  and  the 
control/display  panels.  The  helmet  is  located  above  the  operator  and  is 
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mounted  on  a  gimbal  transducer  system.  This  sytem  converts  head  movements 
into  tilt  and  pan  commands  used  by  the  central  controller  to  control  the 
BAT'S  main  video  camera.  This  helmet  may  be  swung  out  of  the  way  and  back¬ 
up  camera  controls  on  the  control/display  panels  used  instead.  The  master 
is  a  five  degree-of-freedom  kinematic  duplicate  of  the  right  arm  on  the 
BAT.  The  operator  straps  his/her  arm  on  the  master  arm  and  positions  it 
to  where  he/she  would  like  the  BAT's  right  arm  to  go.  The  positions  of 
each  joint  of  the  master  arm  are  read  by  the  central  controller,  and  trans¬ 
mitted  through  the  joint/actuator  control  system  to  the  BAT's  right  arm. 
Both  the  master  arm  and  helmet  systems  implement  master-slave  control  for 
the  right  arm  and  video  subsystems,  respectively. 

There  are  four  control/display  panels  (see  Figure  A. 10)  situated  in 
front  of  the  operator.  The  functions  of  all  the  controls  on  these  panels 
can  be  changed  through  the  software  of  the  central  controller  (described 
in  the  next  section).  The  top  panel  contains  back-up  controls  for  the 
propulsion  control  of  the  BAT  and  the  back-up  joystick  control  for  the  tilt 
and  pan  of  the  video  subsystem.  The  second  panel  from  the  top  contains  5 
knobs  which  can  operate  in  two  modes.  In  one  mode,  the  knobs  can  tune  the 
gains  and  positions  of  the  joint/actuator  control  system.  In  the  other 
mode,  the  knobs  can  control  the  five  joints  of  the  BAT's  right  arm.  The 
modes  can  be  switched  using  the  software  in  the  central  controller.  There 
is  also  a  back-up  switch  that  can  control  the  opening  and  closing  the  the 
end  effector  of  the  BAT's  right  arm.  A  lever  is  situated  on  the  right  of 
the  panel  to  control  the  motor  for  the  left  arm  roller.  The  third  panel 
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FIGURE  A. 10  ICS  Control  Panels 
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from  the  top  is  the  largest  and  contains  the  video  monitors  for  the  ICS. 
There  are  2  nine  inch  monitors  and  4  four  inch  monitors  above  them.  On 
the  panel  is  a  dial  which  is  used  to  switch  the  display  around  on  the 
different  monitors.  These  monitors  can  display  images  sent  from  the  cameras 
on  the  BAT  or  graphics  produced  by  the  software  of  the  central  controller. 
The  graphics  display  is  used  to  show  the  system  settings  and  the  current 
status.  Other  cameras,  such  as  payload  bay  cameras,  may  also  be  hooked 
into  the  system  and  shown  on  these  monitors.  Before  being  connected  to 
the  monitors,  the  cameras  are  hooked  into  video  equipment  located  on  a  rack 
behind  the  panels.  Above  the  monitors  on  the  right  side  is  a  set  of  lights 
which  monitor  the  duty  cycle  for  each  actuator  on  the  BAT.  To  the  right  of 
the  panel  are  the  pressure  switches  which  control  the  high  pressure  of  the 
pneumatic  subsystem  of  the  BAT.  The  switches  control  the  right  arm  end 
effector,  the  left  arm  end  effector  and  the  safety  valve.  They  go  directly 
to  the  communications  link  and  bypass  the  central  controller.  Below  the 
video  monitors  are  three  backup  buttons  for  the  three  pressure  switches. 

The  bottom  panel  contains  the  primary  controls  for  the  BAT's  propulsion 
subsystem.  These  two  joysticks  (each  joystick  has  three  degrees-of-freedom) 
are  used  to  control  the  BAT's  movements.  These  controls  can  also  be  changed 
by  software  to  control  the  BAT’s  right  arm.  Four  pushbuttons  are  located 
between  these  joysticks.  These  buttons  are  used  as  backups  for  selecting 
options  in  the  software  (these  options  are  described  in  the  next  section). 
All  the  outputs  from  the  master  arm,  the  helmet  gimbal  system  and  the  panel 
controls  (with  the  exception  of  the  pressure  controls)  go  to  an  analog-to- 
digital  converter  for  processing.  The  analog-to-digital  converter  digitizes 
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and  multiplexes  these  inputs  for  use  by  the  central  controller  software. 

B)  Central  Controller 

The  central  controller  is  located  on  a  rack  behind  the  ICS  control/ 
display  panels.  The  central  controller  is  an  IBM  Personal  Computer  with 
256K  RAM,  two  disk  drives,  a  math  8087  coprocessor  and  special  interface 
boards  to  communicate  to  the  other  subsystems  (see  Figure  A.ll).  The 
central  controller; 

-  interfaces  with  the  controls  and  displays  of  the  ICS 
the  the  analog-to-digital  converter. 

-  commands  and  monitors  the  joint/actuator  and 
propulsion  control  subsystem. 

-  provides  integrated  control  functions. 

-  records  test  data. 

System  status  (such  as  battery  voltage  levels)  is  sent  from  the  control 
box  on  the  BAT  through  the  communications  uplink  to  the  central  controller 
The  software  which  controls  the  BAT  system  is  described  in  detail  in 
Section  A. 4. 

C)  Joint/Actuator  Control  System 

The  joint/actuator  control  subsystem  consists  of  two  parts;  a 
monitor/buffer  card  and  the  joint/actuator  control  cards  (see  Figure  A. 12) 
The  monitor/buffer  card  is  a  standardized  Z-80  microprocessor  card  which 
buffers  the  commands  from  the  central  controller  and  retransmits  them  to 
the  joint/actuator  control  cards.  It  interfaces  the  central  controller 
with  the  joint/actuator  cards.  There  are  four  joint  actuator  cards,  each 
of  which  uses  a  Z-80  microprocessor.  Each  card  implements  closed-loop 
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control  for  two  of  the  BAT's  eight  actuators.  Control  algorithms  and 
parameters  are  downloaded  to  the  cards  from  the  central  controller.  They 
generate  PWM  drive  signals  and  transmit  them  through  the  communications 
downlink  to  the  BAT's  five  joint  actuators  of  the  right  arm  and  the 
actuators  for  the  left  arm  roller  and  for  the  camera  tilt  and  pan.  Each 
of  the  eight  actuator  positions  are  transmitted  through  the  communications 
uplink  back  to  the  cards. 

D)  Propul S’on  Control  System 

The  propulsion  controller  is  implemented  with  a  microprocessor 
card  (see  Figure  A. 13).  The  propulsion  control  card: 

-  accepts  rotation  and  translation  commands  from  the 
central  controller 

-  performs  the  transformations  necessary  to  convert 
rotation  and  translation  commands  to  individual 
thruster  commands. 

-  generates  PWM  drive  signals  for  the  power  modules 
of  the  Propulsion  subsystem. 

-  receives  feedback  from  the  BAT's  inertial  package 
through  the  communications  uplink. 

E)  Communication  Link 

The  communication  link  is  a  two  megabits  per  second  link  which 
connects  the  control  system  to  the  BAT  (see  Figure  A. 14).  The  communica¬ 
tion  link  consists  of  three  parts;  the  downlink  digital  multiplexer,  the 
uplink  digital  demultiplexer  and  the  analog  transmitter  and  receiver.  The 
BAT  has  a  similar  link  on-board.  The  speed  of  the  communications  link  can 
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FIGURE  A. 13  Propulsion  Control  System 
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FIGURE  A. 14  Communication  Link 
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be  upgraded  if  necessary.  The  digital  multiplexer  and  demultiplexer  are 
on  separate  cards  in  the  control  station  whereas  they  are  on  the  one  card 
in  the  BAT.  The  analog  receiver  and  transmitter  are  combined  into  a 
transceiver. 

The  hardware  of  the  ICS  and  the  BAT  combine  to  make  an  effective 
teleoperator  system.  The  interface  between  the  operator  and  the  system  is 
in  the  software.  The  software  can  be  used  to  change  much  of  the  way  this 
interface  occurs. 

A. 3  Software  of  the  ICS 

The  central  controller  interfaces  the  ICS  controls  with  the  action 
and  support  subsystems  of  the  BAT.  The  vast  majority  of  the  central 
controller  software  is  written  in  the  C  programming  language.  The  rest  of 
the  software  is  in  assembly  language.  The  modular  aspect  of  C  makes  the 
software  easier  to  follow  and  debug.  Including  the  HAIN()  function,  the 
software  is  composed  of  34  functions  and  the  standard  functions  included 
in  the  C  compiler.  The  34  functions  are  all  contained  in  three 

files;  BATl.C,  SUPPORTl.C  and  SUPP0RT2.C.  These  three  files  are  linked 
together  to  make  up  the  operating  program  for  the  BAT  system. 

The  three  files  contain  all  the  software  to  start  up,  run,  adjust 
and  shut-down  the  BAT  system.  It  also  displays  all  the  necessary  informa¬ 
tion  on  a  monitor  at  the  ICS.  The  file  BATl.C  contains  the  MAIN()  function 
which  calls  everything  else  (see  Figure  A. 15). 

BATl .C  calls: 

A)  ini tial izebat( ) 

B)  cal ibratebat( ) 
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FIGURE  A. 15  Main()  Function 
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C)  screenO  and  screen  routines 

D)  runbatO 

E)  shutdownbat( ) 

SUPPORTl.C  and  SUPP0RT2.C  contain 

functions  which  are  called  from  these  five  main  functions. 

A)  Initial izebat( )  (see  Figure  A. 16) 

This  function  configures  the  ICS  interface  and  initializes  the  joint/ 
actuator  and  propulsion  control  subsystem.  It  first  calls  batportsO 
which  configures  the  ports  on  the  interface  board  in  the  central  controller 
BatportsO  does  this  by  calling  setport()  for  each  of  the  five  Intel  8255 
Programmable  Parallel  Interface  chips  on  the  board.  Setport()  sets  an 
8255 's  port  to  the  input  or  output  mode.  It  calls  outportO  to  send  out 
the  address  and  the  control  word  to  the  cl  <p.  OutportO  and  inportO  are 
functions  which  are  written  in  Assembly  language  for  speed  and  both  do 
precisely  what  their  names  imply.  They  output  information  to  a  desired 
port  from  the  central  controller,  and  input  information  from  a  desired 
port  to  the  central  controller.  After  the  ports  are  configured,  initialize 
batO  calls  outportO  to  initialize  the  joint/actuator,  video  and  propul¬ 
sion  control  systems.  It  does  this  by  either  outputting  zero  to  the  port 
or  strobing  the  port.  The  main  power  relay  is  then  checked  by  inportO  to 
see  if  the  relay  has  been  activated.  If  the  main  power  relay  is  activated, 
the  ope'^ator  is  notified  and  must  deactivate  the  switch.  The  initializa¬ 
tion  process  is  then  complete  and  control  is  returned  to  KAINO. 

B)  Cal  ibratebatO  (see  Figure  A. 17) 

This  function  calibrates  the  two  3  degrees-of-freedom  joysticks  used 
for  primary  control  of  the  Propulsion  subsystem.  It  informs  the 
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E)  ShutdownbatO  (see  Figure  A. 20) 

This  function  simply  shuts  down  the  BAT  system  by  strobing  the 
buffer/monitor  and  propulsion  controller  cards.  It  then  returns  to  the 
MAIN()  function.  The  MAIN()  function  then  calls  a  screen  routine  and 
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actuator  cards  from  a  file.  The  screen()  function  is  then  called  to  re¬ 
store  the  original  display.  Control  is  then  returned  to  the  beginning  of 
the  main  loop. 

i)  'i'  -  calls  adjustpotsO .  This  function  uses  the  convertO 

and  adjustO  functions  to  adjust  the  potentiometers  used  for  control  t^- 
puts  on  the  ICS.  It  can  change  the  minimum  or  maximum  limits,  the  of  sets 
or  the  interface  between  potentiometers  and  the  ICS.  It  then  calls  the 
screenO  function  to  restore  the  original  display  and  returns  control 

to  the  beginning  of  the  main  loop. 

j)  'b'  -  this  option  is  still  being  developed.  It  calls  many 
functions  such  as  testroutineO ,  readmast(),  readjm().  Its  purpose  is  to 
test  the  feedback  of  the  joint/actuator  control  system.  It  attempts  to 
bring  the  feedback  to  the  central  controller.  Currently  the  feedback  stops 
at  the  joint/actuator  cards.  It  then  restores  the  display  using  screen() 
and  returns  control  to  the  beginning  of  the  main  loop. 

k)  ‘1,2, 3, 4'  -  these  four  options  call  shut  cardoff().  This 
function  shuts  down  the  card  which  is  selected.  It  sets  the  card's  values 
to  zero  and  calls  transmast().  By  shutting  down  the  card,  it  also  turns 
off  that  joint/actuator.  It  then  returns  control  to  the  beginning  of  the 
main  loop. 

6)  After  these  options  are  selected  (or  not  selected  as  in  the 
first  case),  synchronization  is  tested  and  the  loop  begins  again.  It  does 
not  exit  the  loop  until  carriage  return  is  selected. 
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the  propulsion  card  are  reset  using  outport().  Initial  values  for 
address,  data  and  command  are  sent  to  the  buffer/monitor  card  via 
transmastO.  Control  is  then  returned  to  the  beginning  of  the  main 
loop. 

f)  'c*  -  changes  the  camera  control  system.  This  is  done  by 

switching  the  value  of  the  variable  "headcontrol ler"  (which  was  initialized 
as  true).  If  the  variable  is  true,  the  helmet  gimbal  system  controls  the 
video  camera  on  the  BAT.  If  it  is  false,  the  backup  joystick  on  the  ICS 
would  then  control  the  camera  (Chapter  3,  Figure  3.2).  Control  is  then 
returned  to  the  beginning  of  the  main  loop. 

g)  'p'  -  switches  the  value  of  "puma".  This  variable 

determines  whether  the  propulsion  system  is  on  or  off.  Control  is  then 
returned  to  the  beginning  of  the  main  loop. 

h)  't'  -  calls  setupjointsO.  Setupjoints()  clears  the  screen, 

puts  up  a  menu  and  retrieves  a  selection  from  the  operator.  The 
operator  determines  how  he/she  would  like  the  joint  control  parameters 
set  up.  It  gives  the  operator  the  capability  of  changing  the  gain  of  the 
controller,  parameters  of  the  controller  and  the  type  of  control  algorithm 
on  the  joint/actuator  card  (Proportional,  Proportional-Derivative,  State 
Feedback).  It  does  this  for  each  of  the  joint/actuators  in  the  system  by 
calling  tunejoint().  Once  the  operator  is  satisfied  with  the  values 
selected  for  each  joint,  the  function  setupjointsO  writes  the  values  to  a 
file.  The  operator  can  also  select  the  loadfromf ile( )  function.  This 
function  loads  the  values  (determined  at  some  other  time)  of  the  joint/ 


-142- 


putted  from  the  keyboard  and  returns  its  value.  If  no  character  has  been 
inputted,  it  returns  zero.  A  menu  is  printed  on  the  screen  to  help  the 
operator  select  an  option.  The  different  options  are  outlined  below. 

a)  No  input  -  if  there  is  no  input  (i.e.,  the  operator  does  nothing), 
the  central  controller  calls  inport()  to  check  the  status  of  five  switches. 
These  switches  are  located  between  the  two  joysticks  on  the  bottom  panel 
(see  Chapter  3,  Figure  3.2).  Currently,  since  these  switches  have  no 
function,  the  central  controller  simply  checks  if  the  switch  settings  were 
changed.  If  the  settings  were  changed,  the  central  controller  prints  a 
number  corresponding  to  the  switch  settings  and  returns  to  the  beginning 

of  the  main  loop  (checking  synchronization).  If  the  switches  were  not 
changed  it  just  returns  to  the  beginning  of  the  main  loop. 

b)  Carriage  return  (value  equals  13)  -  breaks  out  of  the  main 

loop  and  goes  to  shutdownbat( ) .  It  shuts  down  the  system. 

c)  's’  -  switches  the  value  of  "joyflag".  Switching  is  defined 

as  whatever  the  variable  was,  it  is  now  the  opposite  (i.e.,  if  the 
variable  was  true  it  is  changed  to  false).  It  controls  whether  the  joy¬ 
sticks  are  used  to  control  the  propulsion  subsystem  or  the  BAT's  right 
arm.  Control  is  then  returned  to  the  beginning  of  the  main  loop. 

d)  'd'  -  switches  the  value  of  "draw".  This  determines  whether 
or  not  the  operator  wants  the  actuator  and  thruster  levels  displayed  on 
the  screen.  Control  is  then  returned  to  the  beginning  of  the  main  loop. 

e)  'r'  -  resets  the  control  system.  It  reinitializes  the  system 

as  it  did  prior  to  starting  the  main  loop.  The  buffer/monitor  card  and 
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played  on  the  screen  using  the  values  stored  by  either  the  master  arm  or 
the  joysticks.  If  "draw"  is  false  (the  default)  then  this  step  is 
skipped.  The  values  stored  are  then  sent  to  the  buffer/monitor  card  by 
transmast( ) . 

4)  The  propulsion  subsystem  then  follows  a  similar  procedure.  If 
"joyflag"  is  true,  the  convert()  function  reads  the  analog  inputs  of  the 
joysticks.  The  joystick  offsets  are  then  added  and  the  values  are  adjusted. 
The  values  are  stored  in  an  array.  If  "joyflag:  is  false  then  the  convertO 
function  reads  the  backup  controls  on  the  ICS.  After  these  values  are  ad¬ 
justed  they  are  stored  in  an  array.  As  before,  the  variable  "draw"  is 
checked.  If  "draw"  is  true,  the  thruster  levels  are  displayed  using  the 
values  stored.  If  "draw"  is  false  this  step  is  skipped.  The  variable 
"puma"  is  then  checked  to  see  if  the  propulsion  subsystem  is  on.  If  "puma" 
is  true,  the  propulsion  system  is  on  and  the  values  stored  in  the  array  are 
transmitted  to  the  propulsion  control  card  using  transpuma().  Transpuma() 
is  the  analog  of  transmast().  It  insures  the  propulsion  control  card  is 
ready  and  performs  all  the  handshaking  necessary  to  transmit  data.  It 

then  transmits  the  data  to  the  propulsion  control  card.  It  utilizes  the 
inportO  and  outport()  functions.  If  "puma"  is  false  (the  default),  it 
does  not  transmit  the  array  values  and  goes  to  the  next  step.  The  out- 
port  function  is  then  called  to  signal  the  end  of  this  part  of  the  main 
loop.  The  timing  test  is  used  to  see  how  long  the  system  is  working  in 
the  action  loop  and  how  long  it  spends  in  the  option  loop  described  below. 

5)  The  option  loop  is  started  by  utilizing  a  screen  routine 
supplied  by  the  compiler.  The  routine  (scrcstsO)  reads  a  character  in- 
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there  are  no  hung  joint  cards  and  the  propulsion  card  is  ready.  Upon  a 
response  from  the  stat()  function,  the  action  part  of  the  loop  begins. 

The  outportO  function  is  called  to  start  a  timing  test.  This  test  is  to 
see  how  long  the  central  controller  is  in  this  part  of  the  main  loop.  The 
action  part  of  the  loop  controls  the  right  arm  and  the  propulsion  subsystem 
by  converting  the  control  inputs  to  commands  and  transmitting  them  to  the 
appropriate  subsystem. 

3)  The  action  part  of  the  loop  begins  with  the  control  of  the 
right  arm.  The  variable  "joyflag"  is  first  tested.  It  can  be  true  or 
false  (it  is  initially  set  to  true).  If  "joyflag"  is  true,  the  master 
arm  on  the  ICS  controls  the  BAT's  right  arm  and  the  two  joysticks  control 
the  propulsion  subsystem.  If  "joyflag"  is  false  the  two  joysticks  control 
the  bat's  right  arm  and  the  backup  controls  on  the  ICS  control  the  propul¬ 
sion  subsystem.  If  the  master  arm  controls  the  BAT's  arm,  the  central 
controller  first  uses  the  convert()  function  to  read  and  convert  the 
analog  inputs  from  the  master  arm.  It  then  uses  the  adjust()  and  align() 
functions  to  insure  that  maximum  and  minimum  limits  of  the  pots  are  not 
exceeded  and  that  the  pot  offsets  are  included.  These  values,  containing 
the  address,  data  and  commands  of  each  joint/actuator  are  stored.  If 
"joyflag"  is  flase,  the  convert()  function  is  used  to  read  the  analog 
inputs  from  the  two  3  degrees-of-freedom  joysticks.  The  joysticks'  off¬ 
sets,  computed  in  cal ibratebatO ,  are  added  and  the  values  are  then  adjust¬ 
ed.  The  values  for  the  address,  data  and  commands  for  each  joint/actuator 
are  stored.  If  the  variable  "draw"  is  true,  the  actuator  levels  are  dis- 
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-  the  master  arm  to  control  the  right  arm 

-  the  two  3  degrees-of-freedom  joysticks  to 
control  the  propulsion  subsystem 

-  the  propulsion  system  to  off 

-  the  helmet  gimbal  system  to  control  the  video 
subsystem 

-  the  display  of  the  actuator  and  thruster  levels  to  off. 

It  initializes  the  buffer  in  the  central  controller  for  the  control  of 
the  bat's  right  arm.  It  does  this  by  initializing  three  arrays  for  the 
address,  data  and  command  of  each  of  the  joint/actuators  on  the  BAT.  It 
sends  these  arrays  to  the  buffer/monitor  card  by  using  the  function 
transmastO.  This  function  is  used  anytime  any  information  must  be  passed 
from  the  central  controller  to  the  buffer/monitor  card.  It  insures  that 
the  buffer  is  not  full  and  that  a  joint  card  is  not  hung  up.  It  performs 
all  handshaking  tasks  needed  to  transmit  the  arrays.  Inport()  and  outputO 
are  used  throughout  the  function.  After  the  buffer/monitor  card  is 
initialized,  the  main  loop  of  runbat()  is  executed. 

2)  Controlling  the  main  loop  is  a  synchronization  function 
(called  synchronizeO  )  .  It  does  precisely  what  its  name  implies  and  is 
tested  at  the  start  of  each  loop.  If  the  system  is  synchronized  it 
continues;  if  it  is  not  synchronized  it  exits  the  loop  and  goes  to 
shutdownbat( ) .  After  the  synchronization  test,  the  status  of  the  buffer/ 
monitor  card  and  the  propulsion  control  card  are  checked  using  the  stat() 
function.  This  function  insures  that  the  buffer/monitor  card  is  ready. 
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operator  to  position  the  joysticks  in  the  center.  After  waiting  for  this 
to  be  done,  it  then  uses  convert()  for  each  degree-of-freedom  on  each  joy¬ 
stick.  ConvertO  takes  an  analog  input  to  the  central  controller's  analog- 
to-digital  converter  and  converts  it  to  an  integer  value.  With  the  joy¬ 
sticks  at  the  center  position  and  the  integer  value  returned  by  convert(), 
the  central  controller  computes  and  stores  an  offset  for  each  of  the  joy¬ 
stick's  degrees-of- freedom.  Control  is  then  returned  to  MAIN(). 

C)  screenO  (see  Figure  A. 18) 

This  function  sets  up  the  video  background  for  the  displays  created 
by  the  central  controller.  It  calls  screen  routines  and  functions  (such  as 
printfO  which  are  implemented  in  this  version  of  the  compiler.  The  screen() 
function  is  also  called  by  runbat().  RunbatO  uses  some  functions  which 
change  the  display.  After  these  functions  are  done,  they  call  screen()  to 
put  up  the  original  display.  Control  is  returned  to  the  calling  function 
after  the  display  is  on  the  screen.  Screen  routines  and  input/output 
functions  supplied  by  the  compiler  are  called  throughout  the  BAT  system's 
software.  These  routines  and  the  screen()  function  provide  the  operator 
with  a  continuous  display  of  the  BAT  system  operation.  The  screen  routines 
also  provide  another  method  for  the  operator  to  interact  with  the  software. 

D)  runbatO  (see  Figure  A. 19) 

1)  After  the  initialization  and  calibration  functions  are  completed 
and  the  display  is  set  up,  the  BAT  system  is  ready  for  operation.  The 
runbatO  function  first  initializes  t*''  minimum  and  maximum  positions  of 
the  potentiometers  on  the  controls  and  configures  the  initial  controls  of 


the  ICS.  It  sets: 


FIGURE  A. 17  Calibrate  Bat()  Funct 
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FIGURE  A. 16  Initialize  BAT()  Function 
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